From 16a26a47abfe95e9fc4d240a2c03a3d9483037d3 Mon Sep 17 00:00:00 2001 From: Jerry Xu Date: Wed, 15 May 2024 21:29:54 -0400 Subject: [PATCH] config changed --- .../legged_gym/envs/a1/a1_crawl_config.py | 2 +- .../legged_gym/envs/a1/a1_down_config.py | 2 +- .../legged_gym/envs/a1/a1_field_config.py | 69 ++-- .../legged_gym/envs/a1/a1_field_config_new.py | 302 ++++++++++++++++++ .../envs/a1/a1_field_distill_config.py | 2 +- .../legged_gym/envs/a1/a1_jump_config.py | 2 +- .../legged_gym/envs/a1/a1_leap_config.py | 2 +- .../legged_gym/envs/a1/a1_remote_config.py | 2 +- .../legged_gym/envs/a1/a1_tilt_config.py | 2 +- .../legged_gym/envs/base/legged_robot.py | 2 +- legged_gym/legged_gym/scripts/play.py | 4 +- .../legged_gym/utils/terrain/barrier_track.py | 2 +- 12 files changed, 335 insertions(+), 58 deletions(-) create mode 100644 legged_gym/legged_gym/envs/a1/a1_field_config_new.py diff --git a/legged_gym/legged_gym/envs/a1/a1_crawl_config.py b/legged_gym/legged_gym/envs/a1/a1_crawl_config.py index 9f163f8..2c5b9c4 100644 --- a/legged_gym/legged_gym/envs/a1/a1_crawl_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_crawl_config.py @@ -1,5 +1,5 @@ import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1CrawlCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_down_config.py b/legged_gym/legged_gym/envs/a1/a1_down_config.py index c671cb3..f6cc8aa 100644 --- a/legged_gym/legged_gym/envs/a1/a1_down_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_down_config.py @@ -1,6 +1,6 @@ from os import path as osp import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1DownCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_field_config.py b/legged_gym/legged_gym/envs/a1/a1_field_config.py index 057e792..99eca62 100644 --- a/legged_gym/legged_gym/envs/a1/a1_field_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_field_config.py @@ -1,5 +1,4 @@ import numpy as np -import os.path as osp from legged_gym.envs.a1.a1_config import A1RoughCfg, A1RoughCfgPPO class A1FieldCfg( A1RoughCfg ): @@ -38,12 +37,12 @@ class A1FieldCfg( A1RoughCfg ): resolution = [16, 16] position = [0.26, 0., 0.03] # position in base_link rotation = [0., 0., 0.] # ZYX Euler angle in base_link - + class proprioception: delay_action_obs = False latency_range = [0.0, 0.0] latency_resample_time = 2.0 # [s] - + class terrain( A1RoughCfg.terrain ): mesh_type = "trimesh" # Don't change num_rows = 20 @@ -60,7 +59,7 @@ class A1FieldCfg( A1RoughCfg ): BarrierTrack_kwargs = dict( options= [ - # "jump", + # "climb", # "crawl", # "tilt", # "leap", @@ -69,11 +68,11 @@ class A1FieldCfg( A1RoughCfg ): track_block_length= 2., # the x-axis distance from the env origin point wall_thickness= (0.04, 0.2), # [m] wall_height= -0.05, - jump= dict( + climb= dict( height= (0.2, 0.6), depth= (0.1, 0.8), # size along the forward axis fake_offset= 0.0, # [m] an offset that make the robot easier to get into the obstacle - jump_down_prob= 0., # probability of jumping down use it in non-virtual terrain + climb_down_prob= 0.0, ), crawl= dict( height= (0.25, 0.5), @@ -98,17 +97,16 @@ class A1FieldCfg( A1RoughCfg ): virtual_terrain= False, draw_virtual_terrain= True, engaging_next_threshold= 1.2, - engaging_finish_threshold= 0., curriculum_perlin= False, - no_perlin_threshold= 0.1, + no_perlin_threshold= 0.0, ) TerrainPerlin_kwargs = dict( - zScale= [0.08, 0.15], - # zScale= 0.15, # Use a constant zScale for training a walk policy + zScale= [0.1, 0.15], + # zScale= 0.1, # Use a constant zScale for training a walk policy frequency= 10, ) - + class commands( A1RoughCfg.commands ): heading_command = False resampling_time = 10 # [s] @@ -116,6 +114,9 @@ class A1FieldCfg( A1RoughCfg ): lin_vel_x = [-1.0, 1.0] lin_vel_y = [0.0, 0.0] ang_vel_yaw = [0., 0.] + ######## configs for training a walk policy ######### + # lin_vel_y = [-1., 1.] + # ang_vel_yaw = [-1., 1.] class control( A1RoughCfg.control ): stiffness = {'joint': 50.} @@ -124,15 +125,10 @@ class A1FieldCfg( A1RoughCfg ): torque_limits = 25 # override the urdf computer_clip_torque = True motor_clip_torque = False - # action_delay = False # set to True to enable action delay in sim - # action_delay_range = [0.002, 0.022] # [s] - # action_delay_resample_time = 5.0 # [s] class asset( A1RoughCfg.asset ): - penalize_contacts_on = ["base", "thigh", "calf"] + penalize_contacts_on = ["base", "thigh"] terminate_after_contacts_on = ["base", "imu"] - front_hip_names = ["FR_hip_joint", "FL_hip_joint"] - rear_hip_names = ["RR_hip_joint", "RL_hip_joint"] class termination: # additional factors that determines whether to terminates the episode @@ -149,8 +145,8 @@ class A1FieldCfg( A1RoughCfg ): tilt_threshold= 1.5, ) pitch_kwargs = dict( - threshold= 1.6, # [rad] # for leap, jump - jump_threshold= 1.6, + threshold= 1.6, # [rad] # for leap, climb + climb_threshold= 1.6, leap_threshold= 1.5, ) z_low_kwargs = dict( @@ -180,7 +176,7 @@ class A1FieldCfg( A1RoughCfg ): added_mass_range = [1.0, 3.0] randomize_friction = True - friction_range = [0., 2.] + friction_range = [0.0, 1.] init_base_pos_range = dict( x= [0.2, 0.6], @@ -199,34 +195,15 @@ class A1FieldCfg( A1RoughCfg ): # penalty for hardware safety exceed_dof_pos_limits = -1e-1 exceed_torque_limits_i = -2e-1 - soft_dof_pos_limit = 0.01 + soft_dof_pos_limit = 0.9 class normalization( A1RoughCfg.normalization ): class obs_scales( A1RoughCfg.normalization.obs_scales ): forward_depth = 1. - base_pose = [0., 0., 0., 1., 1., 1.] + base_pose = [0., 0., 1., 1., 1., 1.] engaging_block = 1. - """ The following action clip is used for tanh policy activation. """ - # clip_actions_method = "hard" - # dof_pos_redundancy = 0.2 - # clip_actions_low = [] - # clip_actions_high = [] - # for sdk_joint_name, sim_joint_name in zip( - # ["Hip", "Thigh", "Calf"] * 4, - # [ # in the order as simulation - # "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", - # "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", - # "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", - # "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", - # ], - # ): # This setting is only for position control. - # clip_actions_low.append( (A1RoughCfg.asset.sdk_dof_range[sdk_joint_name + "_min"] + dof_pos_redundancy - A1RoughCfg.init_state.default_joint_angles[sim_joint_name]) / a1_action_scale ) - # clip_actions_high.append( (A1RoughCfg.asset.sdk_dof_range[sdk_joint_name + "_max"] - dof_pos_redundancy - A1RoughCfg.init_state.default_joint_angles[sim_joint_name]) / a1_action_scale ) - # del dof_pos_redundancy, sdk_joint_name, sim_joint_name # This is not intended to be an attribute of normalization - """ The above action clip is used for tanh policy activation. """ class noise( A1RoughCfg.noise ): - add_noise = False # disable internal uniform +- 1 noise, and no noise in proprioception class noise_scales( A1RoughCfg.noise.noise_scales ): forward_depth = 0.1 base_pose = 1.0 @@ -267,7 +244,6 @@ class A1FieldCfg( A1RoughCfg ): no_moveup_when_fall = False # chosen heuristically, please refer to `LeggedRobotField._get_terrain_curriculum_move` with fixed body_measure_points -logs_root = osp.join(osp.dirname(osp.dirname(osp.dirname(osp.dirname(osp.abspath(__file__))))), "logs") class A1FieldCfgPPO( A1RoughCfgPPO ): class algorithm( A1RoughCfgPPO.algorithm ): entropy_coef = 0.01 @@ -276,13 +252,12 @@ class A1FieldCfgPPO( A1RoughCfgPPO ): class policy( A1RoughCfgPPO.policy ): rnn_type = 'gru' mu_activation = "tanh" - + class runner( A1RoughCfgPPO.runner ): policy_class_name = "ActorCriticRecurrent" experiment_name = "field_a1" - resume = False - - run_name = "".join(["WalkForward", + run_name = "".join(["WalkingBase", + ("_pEnergySubsteps" + np.format_float_scientific(-A1FieldCfg.rewards.scales.legs_energy_substeps, precision=1, exp_digits=1, trim="-") if A1FieldCfg.rewards.scales.legs_energy_substeps != 0 else ""), ("_propDelay{:.2f}-{:.2f}".format( A1FieldCfg.sensor.proprioception.latency_range[0], A1FieldCfg.sensor.proprioception.latency_range[1], @@ -299,4 +274,4 @@ class A1FieldCfgPPO( A1RoughCfgPPO ): resume = False max_iterations = 10000 save_interval = 500 - \ No newline at end of file + diff --git a/legged_gym/legged_gym/envs/a1/a1_field_config_new.py b/legged_gym/legged_gym/envs/a1/a1_field_config_new.py new file mode 100644 index 0000000..057e792 --- /dev/null +++ b/legged_gym/legged_gym/envs/a1/a1_field_config_new.py @@ -0,0 +1,302 @@ +import numpy as np +import os.path as osp +from legged_gym.envs.a1.a1_config import A1RoughCfg, A1RoughCfgPPO + +class A1FieldCfg( A1RoughCfg ): + class env( A1RoughCfg.env ): + num_envs = 4096 # 8192 + obs_components = [ + "proprioception", # 48 + # "height_measurements", # 187 + "base_pose", + "robot_config", + "engaging_block", + "sidewall_distance", + ] + # privileged_use_lin_vel = True # for the possible of setting "proprioception" in obs and privileged obs different + + ######## configs for training a walk policy ############ + # obs_components = [ + # "proprioception", # 48 + # # "height_measurements", # 187 + # # "forward_depth", + # # "base_pose", + # # "robot_config", + # # "engaging_block", + # # "sidewall_distance", + # ] + # privileged_obs_components = [ + # "proprioception", + # # "height_measurements", + # # "forward_depth", + # "robot_config", + # ] + ######## End configs for training a walk policy ############ + + class sensor: + class forward_camera: + resolution = [16, 16] + position = [0.26, 0., 0.03] # position in base_link + rotation = [0., 0., 0.] # ZYX Euler angle in base_link + + class proprioception: + delay_action_obs = False + latency_range = [0.0, 0.0] + latency_resample_time = 2.0 # [s] + + class terrain( A1RoughCfg.terrain ): + mesh_type = "trimesh" # Don't change + num_rows = 20 + num_cols = 50 + selected = "BarrierTrack" # "BarrierTrack" or "TerrainPerlin", "TerrainPerlin" can be used for training a walk policy. + max_init_terrain_level = 0 + border_size = 5 + slope_treshold = 20. + + curriculum = False # for walk + horizontal_scale = 0.025 # [m] + # vertical_scale = 1. # [m] does not change the value in hightfield + pad_unavailable_info = True + + BarrierTrack_kwargs = dict( + options= [ + # "jump", + # "crawl", + # "tilt", + # "leap", + ], # each race track will permute all the options + track_width= 1.6, + track_block_length= 2., # the x-axis distance from the env origin point + wall_thickness= (0.04, 0.2), # [m] + wall_height= -0.05, + jump= dict( + height= (0.2, 0.6), + depth= (0.1, 0.8), # size along the forward axis + fake_offset= 0.0, # [m] an offset that make the robot easier to get into the obstacle + jump_down_prob= 0., # probability of jumping down use it in non-virtual terrain + ), + crawl= dict( + height= (0.25, 0.5), + depth= (0.1, 0.6), # size along the forward axis + wall_height= 0.6, + no_perlin_at_obstacle= False, + ), + tilt= dict( + width= (0.24, 0.32), + depth= (0.4, 1.), # size along the forward axis + opening_angle= 0.0, # [rad] an opening that make the robot easier to get into the obstacle + wall_height= 0.5, + ), + leap= dict( + length= (0.2, 1.0), + depth= (0.4, 0.8), + height= 0.2, + ), + add_perlin_noise= True, + border_perlin_noise= True, + border_height= 0., + virtual_terrain= False, + draw_virtual_terrain= True, + engaging_next_threshold= 1.2, + engaging_finish_threshold= 0., + curriculum_perlin= False, + no_perlin_threshold= 0.1, + ) + + TerrainPerlin_kwargs = dict( + zScale= [0.08, 0.15], + # zScale= 0.15, # Use a constant zScale for training a walk policy + frequency= 10, + ) + + class commands( A1RoughCfg.commands ): + heading_command = False + resampling_time = 10 # [s] + class ranges( A1RoughCfg.commands.ranges ): + lin_vel_x = [-1.0, 1.0] + lin_vel_y = [0.0, 0.0] + ang_vel_yaw = [0., 0.] + + class control( A1RoughCfg.control ): + stiffness = {'joint': 50.} + damping = {'joint': 1.} + action_scale = 0.5 + torque_limits = 25 # override the urdf + computer_clip_torque = True + motor_clip_torque = False + # action_delay = False # set to True to enable action delay in sim + # action_delay_range = [0.002, 0.022] # [s] + # action_delay_resample_time = 5.0 # [s] + + class asset( A1RoughCfg.asset ): + penalize_contacts_on = ["base", "thigh", "calf"] + terminate_after_contacts_on = ["base", "imu"] + front_hip_names = ["FR_hip_joint", "FL_hip_joint"] + rear_hip_names = ["RR_hip_joint", "RL_hip_joint"] + + class termination: + # additional factors that determines whether to terminates the episode + termination_terms = [ + "roll", + "pitch", + "z_low", + "z_high", + # "out_of_track", + ] + + roll_kwargs = dict( + threshold= 0.8, # [rad] + tilt_threshold= 1.5, + ) + pitch_kwargs = dict( + threshold= 1.6, # [rad] # for leap, jump + jump_threshold= 1.6, + leap_threshold= 1.5, + ) + z_low_kwargs = dict( + threshold= 0.15, # [m] + ) + z_high_kwargs = dict( + threshold= 1.5, # [m] + ) + out_of_track_kwargs = dict( + threshold= 1., # [m] + ) + + check_obstacle_conditioned_threshold = True + timeout_at_border = False + + class domain_rand( A1RoughCfg.domain_rand ): + randomize_com = True + class com_range: + x = [-0.05, 0.15] + y = [-0.1, 0.1] + z = [-0.05, 0.05] + + randomize_motor = True + leg_motor_strength_range = [0.9, 1.1] + + randomize_base_mass = True + added_mass_range = [1.0, 3.0] + + randomize_friction = True + friction_range = [0., 2.] + + init_base_pos_range = dict( + x= [0.2, 0.6], + y= [-0.25, 0.25], + ) + + push_robots = False + + class rewards( A1RoughCfg.rewards ): + class scales: + tracking_ang_vel = 0.05 + world_vel_l2norm = -1. + legs_energy_substeps = -2e-5 + legs_energy = -0. + alive = 2. + # penalty for hardware safety + exceed_dof_pos_limits = -1e-1 + exceed_torque_limits_i = -2e-1 + soft_dof_pos_limit = 0.01 + + class normalization( A1RoughCfg.normalization ): + class obs_scales( A1RoughCfg.normalization.obs_scales ): + forward_depth = 1. + base_pose = [0., 0., 0., 1., 1., 1.] + engaging_block = 1. + """ The following action clip is used for tanh policy activation. """ + # clip_actions_method = "hard" + # dof_pos_redundancy = 0.2 + # clip_actions_low = [] + # clip_actions_high = [] + # for sdk_joint_name, sim_joint_name in zip( + # ["Hip", "Thigh", "Calf"] * 4, + # [ # in the order as simulation + # "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + # "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + # "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", + # "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", + # ], + # ): # This setting is only for position control. + # clip_actions_low.append( (A1RoughCfg.asset.sdk_dof_range[sdk_joint_name + "_min"] + dof_pos_redundancy - A1RoughCfg.init_state.default_joint_angles[sim_joint_name]) / a1_action_scale ) + # clip_actions_high.append( (A1RoughCfg.asset.sdk_dof_range[sdk_joint_name + "_max"] - dof_pos_redundancy - A1RoughCfg.init_state.default_joint_angles[sim_joint_name]) / a1_action_scale ) + # del dof_pos_redundancy, sdk_joint_name, sim_joint_name # This is not intended to be an attribute of normalization + """ The above action clip is used for tanh policy activation. """ + + class noise( A1RoughCfg.noise ): + add_noise = False # disable internal uniform +- 1 noise, and no noise in proprioception + class noise_scales( A1RoughCfg.noise.noise_scales ): + forward_depth = 0.1 + base_pose = 1.0 + + class viewer( A1RoughCfg.viewer ): + pos = [0, 0, 5] # [m] + lookat = [5., 5., 2.] # [m] + + draw_volume_sample_points = False + + class sim( A1RoughCfg.sim ): + body_measure_points = { # transform are related to body frame + "base": dict( + x= [i for i in np.arange(-0.2, 0.31, 0.03)], + y= [-0.08, -0.04, 0.0, 0.04, 0.08], + z= [i for i in np.arange(-0.061, 0.061, 0.03)], + transform= [0., 0., 0.005, 0., 0., 0.], + ), + "thigh": dict( + x= [ + -0.16, -0.158, -0.156, -0.154, -0.152, + -0.15, -0.145, -0.14, -0.135, -0.13, -0.125, -0.12, -0.115, -0.11, -0.105, -0.1, -0.095, -0.09, -0.085, -0.08, -0.075, -0.07, -0.065, -0.05, + 0.0, 0.05, 0.1, + ], + y= [-0.015, -0.01, 0.0, -0.01, 0.015], + z= [-0.03, -0.015, 0.0, 0.015], + transform= [0., 0., -0.1, 0., 1.57079632679, 0.], + ), + "calf": dict( + x= [i for i in np.arange(-0.13, 0.111, 0.03)], + y= [-0.015, 0.0, 0.015], + z= [-0.015, 0.0, 0.015], + transform= [0., 0., -0.11, 0., 1.57079632679, 0.], + ), + } + + class curriculum: + no_moveup_when_fall = False + # chosen heuristically, please refer to `LeggedRobotField._get_terrain_curriculum_move` with fixed body_measure_points + +logs_root = osp.join(osp.dirname(osp.dirname(osp.dirname(osp.dirname(osp.abspath(__file__))))), "logs") +class A1FieldCfgPPO( A1RoughCfgPPO ): + class algorithm( A1RoughCfgPPO.algorithm ): + entropy_coef = 0.01 + clip_min_std = 1e-12 + + class policy( A1RoughCfgPPO.policy ): + rnn_type = 'gru' + mu_activation = "tanh" + + class runner( A1RoughCfgPPO.runner ): + policy_class_name = "ActorCriticRecurrent" + experiment_name = "field_a1" + resume = False + + run_name = "".join(["WalkForward", + ("_propDelay{:.2f}-{:.2f}".format( + A1FieldCfg.sensor.proprioception.latency_range[0], + A1FieldCfg.sensor.proprioception.latency_range[1], + ) if A1FieldCfg.sensor.proprioception.delay_action_obs else "" + ), + ("_aScale{:d}{:d}{:d}".format( + int(A1FieldCfg.control.action_scale[0] * 10), + int(A1FieldCfg.control.action_scale[1] * 10), + int(A1FieldCfg.control.action_scale[2] * 10), + ) if isinstance(A1FieldCfg.control.action_scale, (tuple, list)) \ + else "_aScale{:.1f}".format(A1FieldCfg.control.action_scale) + ), + ]) + resume = False + max_iterations = 10000 + save_interval = 500 + \ No newline at end of file diff --git a/legged_gym/legged_gym/envs/a1/a1_field_distill_config.py b/legged_gym/legged_gym/envs/a1/a1_field_distill_config.py index 6607bdf..46af5f3 100644 --- a/legged_gym/legged_gym/envs/a1/a1_field_distill_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_field_distill_config.py @@ -3,7 +3,7 @@ import os import os.path as osp from datetime import datetime import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1FieldDistillCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_jump_config.py b/legged_gym/legged_gym/envs/a1/a1_jump_config.py index fd09c1c..51953ac 100644 --- a/legged_gym/legged_gym/envs/a1/a1_jump_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_jump_config.py @@ -1,6 +1,6 @@ from os import path as osp import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1JumpCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_leap_config.py b/legged_gym/legged_gym/envs/a1/a1_leap_config.py index 59076ec..3b1ca9b 100644 --- a/legged_gym/legged_gym/envs/a1/a1_leap_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_leap_config.py @@ -1,5 +1,5 @@ import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1LeapCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_remote_config.py b/legged_gym/legged_gym/envs/a1/a1_remote_config.py index 60f3cd4..cb25de9 100644 --- a/legged_gym/legged_gym/envs/a1/a1_remote_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_remote_config.py @@ -1,6 +1,6 @@ import numpy as np import os.path as osp -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1RemoteCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/a1/a1_tilt_config.py b/legged_gym/legged_gym/envs/a1/a1_tilt_config.py index d3e6597..738422d 100644 --- a/legged_gym/legged_gym/envs/a1/a1_tilt_config.py +++ b/legged_gym/legged_gym/envs/a1/a1_tilt_config.py @@ -1,5 +1,5 @@ import numpy as np -from legged_gym.envs.a1.a1_field_config import A1FieldCfg, A1FieldCfgPPO +from legged_gym.envs.a1.a1_field_config_new import A1FieldCfg, A1FieldCfgPPO from legged_gym.utils.helpers import merge_dict class A1TiltCfg( A1FieldCfg ): diff --git a/legged_gym/legged_gym/envs/base/legged_robot.py b/legged_gym/legged_gym/envs/base/legged_robot.py index 0513206..7f8537b 100644 --- a/legged_gym/legged_gym/envs/base/legged_robot.py +++ b/legged_gym/legged_gym/envs/base/legged_robot.py @@ -578,7 +578,7 @@ class LeggedRobot(BaseTask): self.all_root_states = gymtorch.wrap_tensor(actor_root_state) self.root_states = self.all_root_states.view(self.num_envs, -1, 13)[:, 0, :] # (num_envs, 13) self.all_dof_states = gymtorch.wrap_tensor(dof_state_tensor) - self.dof_state = self.all_dof_states.view(self.num_envs, -1, 2)[:, :self.num_dof, :] # (num_envs, 2) + self.dof_state = self.all_dof_states.view(self.num_envs, -1, 2)[:, :self.num_dof, :] # (num_envs, 2) position + velocity self.dof_pos = self.dof_state.view(self.num_envs, -1, 2)[..., :self.num_dof, 0] self.dof_vel = self.dof_state.view(self.num_envs, -1, 2)[..., :self.num_dof, 1] self.base_quat = self.root_states[:, 3:7] diff --git a/legged_gym/legged_gym/scripts/play.py b/legged_gym/legged_gym/scripts/play.py index 0a1063d..2eda6a2 100644 --- a/legged_gym/legged_gym/scripts/play.py +++ b/legged_gym/legged_gym/scripts/play.py @@ -107,8 +107,8 @@ def play(args): ] if "one_obstacle_per_track" in env_cfg.terrain.BarrierTrack_kwargs.keys(): env_cfg.terrain.BarrierTrack_kwargs.pop("one_obstacle_per_track") - env_cfg.terrain.BarrierTrack_kwargs["n_obstacles_per_track"] = 2 - env_cfg.commands.ranges.lin_vel_x = [1.2, 1.2] + env_cfg.terrain.BarrierTrack_kwargs["n_obstacles_per_track"] = 0# 2 + env_cfg.commands.ranges.lin_vel_x = [1.2, 1.2] # [1.2, 1.2] if "distill" in args.task: env_cfg.commands.ranges.lin_vel_x = [0.0, 0.0] env_cfg.commands.ranges.lin_vel_y = [-0., 0.] diff --git a/legged_gym/legged_gym/utils/terrain/barrier_track.py b/legged_gym/legged_gym/utils/terrain/barrier_track.py index e439121..9567083 100644 --- a/legged_gym/legged_gym/utils/terrain/barrier_track.py +++ b/legged_gym/legged_gym/utils/terrain/barrier_track.py @@ -750,7 +750,7 @@ class BarrierTrack: block_idx, 0., (self.n_blocks_per_track - 1), - ) + ).to(int) # compute whether the robot is still in any of the track in_track_mask = (track_idx == track_idx_clipped).all(dim= -1) & (block_idx == block_idx_clipped)