Skip to content

yzh / agielf2_amp

转到一个项目
Commit 03eab9af by liufq
Options

1.elf12 good

parent acced63a
正在显示 包含 116 行增加 和 61 行删除
humanoid/envs/__init__.py
......@@ -38,6 +38,12 @@ from .x1.x1_dh_stand_config import X1DHStandCfg, X1DHStandCfgPPO
from .x1.x1_dh_stand_env import X1DHStandEnv
from .elf12.elf12_dh_stand_config import Elf12DHStandCfg, Elf12DHStandCfgPPO
from .elf12.elf12_dh_stand_env import Elf12DHStandEnv
from humanoid.utils.task_registry import task_registry
task_registry.register( "x1_dh_stand", X1DHStandEnv, X1DHStandCfg(), X1DHStandCfgPPO() )
task_registry.register( "elf12_dh_stand", Elf12DHStandEnv, Elf12DHStandCfg(), Elf12DHStandCfgPPO() )
humanoid/envs/elf12/elf12_dh_stand_config.py
......@@ -32,7 +32,7 @@
from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
class X1DHStandCfg(LeggedRobotCfg):
class Elf12DHStandCfg(LeggedRobotCfg):
"""
Configuration class for the XBotL humanoid robot.
"""
......@@ -60,16 +60,16 @@ class X1DHStandCfg(LeggedRobotCfg):
class asset(LeggedRobotCfg.asset):
file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/urdf/x1.urdf'
xml_file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/mjcf/xyber_x1_flat.xml'
file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/elf12/urdf/elf12.urdf'
xml_file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/xxxx'
name = "x1"
foot_name = "ankle_roll"
knee_name = "knee_pitch"
name = "elf12"
foot_name = "ankle_x_link"
knee_name = "knee_y_link"
terminate_after_contacts_on = ['base_link']
penalize_contacts_on = ["base_link"]
self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
terminate_after_contacts_on = ['base_link', 'hip']
penalize_contacts_on = ["base_link", "hip"]
self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter
flip_visual_attachments = False
replace_cylinder_with_capsule = False
fix_base_link = False
......@@ -123,31 +123,30 @@ class X1DHStandCfg(LeggedRobotCfg):
class init_state(LeggedRobotCfg.init_state):
pos = [0.0, 0.0, 0.7]
pos = [0.0, 0.0, 0.78]
default_joint_angles = { # = target angles [rad] when action = 0.0
'left_hip_pitch_joint': 0.4,
'left_hip_roll_joint': 0.05,
'left_hip_yaw_joint': -0.31,
'left_knee_pitch_joint': 0.49,
'left_ankle_pitch_joint': -0.21,
'left_ankle_roll_joint': 0.0,
'right_hip_pitch_joint': -0.4,
'right_hip_roll_joint': -0.05,
'right_hip_yaw_joint': 0.31,
'right_knee_pitch_joint': 0.49,
'right_ankle_pitch_joint': -0.21,
'right_ankle_roll_joint': 0.0,
'l_hip_z_joint': 0.0,
'l_hip_x_joint': 0.0,
'l_hip_y_joint': -0.3,
'l_knee_y_joint': 0.6,
'l_ankle_y_joint': -0.3,
'l_ankle_x_joint': 0.0,
'r_hip_z_joint': 0.0,
'r_hip_x_joint': 0.0,
'r_hip_y_joint': -0.3,
'r_knee_y_joint': 0.6,
'r_ankle_y_joint': -0.3,
'r_ankle_x_joint': 0.0,
}
class control(LeggedRobotCfg.control):
# PD Drive parameters:
control_type = 'P'
stiffness = {'hip_pitch_joint': 30, 'hip_roll_joint': 40,'hip_yaw_joint': 35,
'knee_pitch_joint': 100, 'ankle_pitch_joint': 35, 'ankle_roll_joint': 35}
damping = {'hip_pitch_joint': 3, 'hip_roll_joint': 3.0,'hip_yaw_joint': 4,
'knee_pitch_joint': 10, 'ankle_pitch_joint': 0.5, 'ankle_roll_joint': 0.5}
stiffness = {'hip_z': 150, 'hip_x': 150, 'hip_y': 150, 'knee_y': 150, 'ankle_y': 20, 'ankle_x': 20}
damping = {'hip_z': 3, 'hip_x': 3, 'hip_y': 3, 'knee_y': 3, 'ankle_y': 1, 'ankle_x': 1}
# action scale: target angle = actionScale * action + defaultAngle
action_scale = 0.5
......@@ -183,8 +182,8 @@ class X1DHStandCfg(LeggedRobotCfg):
push_interval_s = 4 # every this second, push robot
update_step = 2000 * 24 # after this count, increase push_duration index
push_duration = [0, 0.05, 0.1, 0.15, 0.2, 0.25] # increase push duration during training
max_push_vel_xy = 0.2
max_push_ang_vel = 0.2
max_push_vel_xy = 0.5
max_push_ang_vel = 0.3
randomize_base_mass = True
added_mass_range = [-3, 3] # base mass rand range, base mass is all fix link sum mass
......@@ -295,7 +294,7 @@ class X1DHStandCfg(LeggedRobotCfg):
sw_switch = True # use stand_com_threshold or not
class ranges:
lin_vel_x = [-0.4, 1.2] # min max [m/s]
lin_vel_x = [-0.4, 0.8] # min max [m/s]
lin_vel_y = [-0.4, 0.4] # min max [m/s]
ang_vel_yaw = [-0.6, 0.6] # min max [rad/s]
heading = [-3.14, 3.14]
......@@ -304,16 +303,18 @@ class X1DHStandCfg(LeggedRobotCfg):
soft_dof_pos_limit = 0.98
soft_dof_vel_limit = 0.9
soft_torque_limit = 0.9
base_height_target = 0.61
foot_min_dist = 0.2
base_height_target = 0.68
foot_min_dist = 0.15
foot_max_dist = 1.0
# final_swing_joint_pos = final_swing_joint_delta_pos + default_pos
final_swing_joint_delta_pos = [0.25, 0.05, -0.11, 0.35, -0.16, 0.0, -0.25, -0.05, 0.11, 0.35, -0.16, 0.0]
final_swing_joint_delta_pos = [0.0, 0.0, -0.16, 0.32, -0.16, 0.0,
0.0, 0.0, -0.16, 0.32, -0.16, 0.0]
target_feet_height = 0.03
target_feet_height_max = 0.06
feet_to_ankle_distance = 0.041
target_feet_height_max = 0.08
feet_to_ankle_distance = 0.04
cycle_time = 0.7
double_suport = 0.4
# if true negative total rewards are clipped at zero (avoids early termination problems)
only_positive_rewards = True
# tracking reward = exp(-error*sigma)
......@@ -341,7 +342,7 @@ class X1DHStandCfg(LeggedRobotCfg):
default_joint_pos = 1.0
orientation = 1.
feet_rotation = 0.3
base_height = 0.2
# base_height = 0.2
base_acc = 0.2
# energy
action_smoothness = -0.002
......@@ -367,7 +368,7 @@ class X1DHStandCfg(LeggedRobotCfg):
clip_actions = 100.
class X1DHStandCfgPPO(LeggedRobotCfgPPO):
class Elf12DHStandCfgPPO(LeggedRobotCfgPPO):
seed = 5
runner_class_name = 'DHOnPolicyRunner' # DWLOnPolicyRunner
......@@ -382,7 +383,7 @@ class X1DHStandCfgPPO(LeggedRobotCfgPPO):
filter_size=[32, 16]
stride_size=[3, 2]
lh_output_dim= 64 #long history output dim
in_channels = X1DHStandCfg.env.frame_stack
in_channels = Elf12DHStandCfg.env.frame_stack
class algorithm(LeggedRobotCfgPPO.algorithm):
entropy_coef = 0.001
......@@ -391,20 +392,20 @@ class X1DHStandCfgPPO(LeggedRobotCfgPPO):
gamma = 0.994
lam = 0.9
num_mini_batches = 4
if X1DHStandCfg.terrain.measure_heights:
lin_vel_idx = (X1DHStandCfg.env.single_num_privileged_obs + X1DHStandCfg.terrain.num_height) * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
if Elf12DHStandCfg.terrain.measure_heights:
lin_vel_idx = (Elf12DHStandCfg.env.single_num_privileged_obs + Elf12DHStandCfg.terrain.num_height) * (Elf12DHStandCfg.env.c_frame_stack - 1) + Elf12DHStandCfg.env.single_linvel_index
else:
lin_vel_idx = X1DHStandCfg.env.single_num_privileged_obs * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
lin_vel_idx = Elf12DHStandCfg.env.single_num_privileged_obs * (Elf12DHStandCfg.env.c_frame_stack - 1) + Elf12DHStandCfg.env.single_linvel_index
class runner:
policy_class_name = 'ActorCriticDH'
algorithm_class_name = 'DHPPO'
num_steps_per_env = 24 # per iteration
max_iterations = 20000 # number of policy updates
max_iterations = 50000 # number of policy updates
# logging
save_interval = 100 # check for potential saves every this many iterations
experiment_name = 'x1_dh_stand'
save_interval = 500 # check for potential saves every this many iterations
experiment_name = 'elf12_dh_stand'
run_name = ''
# load and resume
resume = False
......
humanoid/envs/elf12/elf12_dh_stand_env.py
......@@ -76,7 +76,7 @@ def get_euler_xyz_tensor(quat):
euler_xyz[euler_xyz > np.pi] -= 2 * np.pi
return euler_xyz
class X1DHStandEnv(LeggedRobot):
class Elf12DHStandEnv(LeggedRobot):
'''
X1DHStandEnv is a class that represents a custom environment for a legged robot.
......@@ -157,7 +157,7 @@ class X1DHStandEnv(LeggedRobot):
# right foot stance
stance_mask[:, 1] = sin_pos < 0
# Add double support phase
stance_mask[torch.abs(sin_pos) < 0.1] = 1
stance_mask[torch.abs(sin_pos) < self.cfg.rewards.double_suport] = 1
# stand mask == 1 means stand leg
return stance_mask
......@@ -295,7 +295,7 @@ class X1DHStandEnv(LeggedRobot):
self.ref_dof_pos[:, 10] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[10]
self.ref_dof_pos[:, 11] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[11]
self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0.
self.ref_dof_pos[torch.abs(sin_pos) < self.cfg.rewards.double_suport] = 0.
# if use_ref_actions=True, action += ref_action
self.ref_action = 2 * self.ref_dof_pos
......@@ -610,7 +610,8 @@ class X1DHStandEnv(LeggedRobot):
with the ground. The speed of the foot is calculated and scaled by the contact conditions.
"""
contact = self.contact_forces[:, self.feet_indices, 2] > 5.
foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2)
# foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2)
foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 7:9], dim=2)
rew = torch.sqrt(foot_speed_norm)
rew *= contact
return torch.sum(rew, dim=1)
......@@ -665,8 +666,8 @@ class X1DHStandEnv(LeggedRobot):
on penalizing deviation in yaw and roll directions. Excludes yaw and roll from the main penalty.
"""
joint_diff = self.dof_pos - self.default_joint_pd_target
left_yaw_roll = joint_diff[:, [1,2,5]]
right_yaw_roll = joint_diff[:, [7,8,11]]
left_yaw_roll = joint_diff[:, [0,1,5]]
right_yaw_roll = joint_diff[:, [6,7,11]]
yaw_roll = torch.norm(left_yaw_roll, dim=1) + torch.norm(right_yaw_roll, dim=1)
yaw_roll = torch.clamp(yaw_roll - 0.1, 0, 50)
return torch.exp(-yaw_roll * 100) - 0.01 * torch.norm(joint_diff, dim=1)
......
humanoid/export.sh 0 → 100644
python scripts/export_policy_dh.py --task=elf12_dh_stand #--load_run=<date_time><run_name>
\ No newline at end of file
humanoid/play.sh 0 → 100644
# python scripts/play.py --task=bot_elf_dh_stand #--load_run=<date_time><run_name>
python scripts/play.py --task=elf12_dh_stand #--load_run=<date_time><run_name>
\ No newline at end of file
humanoid/scripts/play.py
......@@ -132,7 +132,7 @@ def play(args):
logger = Logger(env_cfg.sim.dt * env_cfg.control.decimation)
robot_index = 0 # which robot is used for logging
joint_index = 5 # which joint is used for logging
stop_state_log = 1000 # number of steps before plotting states
stop_state_log = 500 # number of steps before plotting states
if RENDER:
camera_properties = gymapi.CameraProperties()
camera_properties.width = 1920
......@@ -193,8 +193,8 @@ def play(args):
'base_height' : env.root_states[robot_index, 2].item(),
'foot_z_l' : env.rigid_state[robot_index,4,2].item(),
'foot_z_r' : env.rigid_state[robot_index,9,2].item(),
'foot_forcez_l' : env.contact_forces[robot_index,4,2].item(),
'foot_forcez_r' : env.contact_forces[robot_index,9,2].item(),
'foot_forcez_l' : env.contact_forces[robot_index,6,2].item(),
'foot_forcez_r' : env.contact_forces[robot_index,12,2].item(),
'base_vel_x': env.base_lin_vel[robot_index, 0].item(),
'command_x': x_vel_cmd,
'base_vel_y': env.base_lin_vel[robot_index, 1].item(),
......@@ -245,6 +245,6 @@ def play(args):
if __name__ == '__main__':
EXPORT_POLICY = False
RENDER = False
FIX_COMMAND = False
FIX_COMMAND = True #False
args = get_args()
play(args)
humanoid/train.sh 0 → 100644
# python scripts/train.py --task=x1_dh_stand --run_name=test #--headless #--resume
# python ./scripts/train.py --task=bot_elf_dh_stand --run_name=test #--num_envs=3 #--resume #--headless
python ./scripts/train.py --task=elf12_dh_stand --run_name=test #--num_envs=3 #--resume #--headless
\ No newline at end of file
humanoid/utils/logger.py
......@@ -74,6 +74,7 @@ class Logger:
# self.plot_process10 = Process(target=self._plot_tn_rms1)
# self.plot_process11 = Process(target=self._plot_tn1)
# self.plot_process12 = Process(target=self._plot_torque_vel1)
self.plot_process13 = Process(target=self._plot_force)
self.plot_process.start()
self.plot_process1.start()
# self.plot_process2.start()
......@@ -87,6 +88,7 @@ class Logger:
# self.plot_process10.start()
# self.plot_process11.start()
# self.plot_process12.start()
self.plot_process13.start()
def _plot(self):
nb_rows = 3
......@@ -128,6 +130,40 @@ class Logger:
# a.legend()
plt.show()
def _plot_force(self):
for key, value in self.state_log.items():
time = np.linspace(0, len(value)*self.dt, len(value))
break
log= self.state_log
fig,ax=plt.subplots(1,1)
if log["foot_forcez_l"]:
forcez_l = np.array(log["foot_forcez_l"])
forcez_l /= 100.0
smothf_l = np.convolve(forcez_l, np.ones(3)/3, mode='same') #'valid'
# smothf_l = forcez_l
ax.plot(time, smothf_l+1.1, label='measured_l', color='y')
if log["foot_forcez_r"]:
forcez_r = np.array(log["foot_forcez_r"])
forcez_r /= 100.0
smothf_r = np.convolve(forcez_r, np.ones(3)/3, mode='same')
# smothf_r = forcez_r
ax.plot(time, smothf_r+1.1, label='measured_r', color='b')
# if log["contact_mask_r"]:
# ax.plot(time, log["contact_mask_r"], label='contact_mask_r', color='b')
# if log["ref_r_3"]:
# ax.plot(time, log["ref_r_3"], label='ref_r_3', color='b')
if log["command_sin"]: ax.plot(time, log["command_sin"], label='commanded_sin', color='y')
if log["command_cos"]: ax.plot(time, log["command_cos"], label='commanded_cos', color='b')
ax.set(xlabel='time [s]', ylabel='command cos', title='Command Cos')
plt.show()
def _plot_position(self):
nb_rows = 2
nb_cols = 3
......
resources/robots/elf12/urdf/elf12.urdf
......@@ -361,7 +361,7 @@
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<!-- <collision>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
......@@ -369,8 +369,8 @@
<mesh
filename="../meshes/l_ankle_x_link.STL" />
</geometry>
</collision> -->
<collision name="l_foot_1">
</collision>
<!-- <collision name="l_foot_1">
<origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
......@@ -381,7 +381,7 @@
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
</collision> -->
</link>
<joint
name="l_ankle_x_joint"
......@@ -721,7 +721,7 @@
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<!-- <collision>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
......@@ -729,8 +729,8 @@
<mesh
filename="../meshes/r_ankle_x_link.STL" />
</geometry>
</collision> -->
<collision name="r_foot_1">
</collision>
<!-- <collision name="r_foot_1">
<origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
......@@ -741,7 +741,7 @@
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
</collision> -->
</link>
<joint
name="r_ankle_x_joint"
......
Markdown 格式
0% 或
您添加了 0 人 到此讨论。请谨慎行事。
请先完成此评论的编辑！
请 注册 或 登录 后发表评论