import isaacgym assert isaacgym import matplotlib.pyplot as plt import torch from tqdm import trange from Go2Py.sim.gym.envs import * from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg from Go2Py.sim.gym.envs.go2.go2_config import config_go2 from Go2Py.sim.gym.envs.go2.velocity_tracking import VelocityTrackingEasyEnv def run_env(render=False, headless=False): # prepare environment config_go2(Cfg) Cfg.commands.num_lin_vel_bins = 30 Cfg.commands.num_ang_vel_bins = 30 Cfg.curriculum_thresholds.tracking_ang_vel = 0.7 Cfg.curriculum_thresholds.tracking_lin_vel = 0.8 Cfg.curriculum_thresholds.tracking_contacts_shaped_vel = 0.9 Cfg.curriculum_thresholds.tracking_contacts_shaped_force = 0.9 Cfg.commands.distributional_commands = True Cfg.env.priv_observe_motion = False Cfg.env.priv_observe_gravity_transformed_motion = True Cfg.domain_rand.randomize_friction_indep = False Cfg.env.priv_observe_friction_indep = False Cfg.domain_rand.randomize_friction = True Cfg.env.priv_observe_friction = False Cfg.domain_rand.friction_range = [0.0, 0.0] Cfg.domain_rand.randomize_restitution = True Cfg.env.priv_observe_restitution = False Cfg.domain_rand.restitution_range = [0.0, 1.0] Cfg.domain_rand.randomize_base_mass = True Cfg.env.priv_observe_base_mass = False Cfg.domain_rand.added_mass_range = [-1.0, 3.0] Cfg.domain_rand.randomize_gravity = True Cfg.domain_rand.gravity_range = [-1.0, 1.0] Cfg.domain_rand.gravity_rand_interval_s = 2.0 Cfg.domain_rand.gravity_impulse_duration = 0.5 Cfg.env.priv_observe_gravity = True Cfg.domain_rand.randomize_com_displacement = False Cfg.domain_rand.com_displacement_range = [-0.15, 0.15] Cfg.env.priv_observe_com_displacement = False Cfg.domain_rand.randomize_ground_friction = True Cfg.env.priv_observe_ground_friction = False Cfg.env.priv_observe_ground_friction_per_foot = False Cfg.domain_rand.ground_friction_range = [0.3, 2.0] Cfg.domain_rand.randomize_motor_strength = True Cfg.domain_rand.motor_strength_range = [0.9, 1.1] Cfg.env.priv_observe_motor_strength = False Cfg.domain_rand.randomize_motor_offset = True Cfg.domain_rand.motor_offset_range = [-0.02, 0.02] Cfg.env.priv_observe_motor_offset = False Cfg.domain_rand.push_robots = False Cfg.domain_rand.randomize_Kp_factor = False Cfg.env.priv_observe_Kp_factor = False Cfg.domain_rand.randomize_Kd_factor = False Cfg.env.priv_observe_Kd_factor = False Cfg.env.priv_observe_body_velocity = False Cfg.env.priv_observe_body_height = False Cfg.env.priv_observe_desired_contact_states = False Cfg.env.priv_observe_contact_forces = False Cfg.env.priv_observe_foot_displacement = False Cfg.env.num_privileged_obs = 3 Cfg.env.num_observation_history = 30 Cfg.reward_scales.feet_contact_forces = 0.0 Cfg.domain_rand.rand_interval_s = 4 Cfg.commands.num_commands = 15 Cfg.env.observe_two_prev_actions = True Cfg.env.observe_yaw = True Cfg.env.num_observations = 71 Cfg.env.num_scalar_observations = 71 Cfg.env.observe_gait_commands = True Cfg.env.observe_timing_parameter = False Cfg.env.observe_clock_inputs = True Cfg.domain_rand.tile_height_range = [-0.0, 0.0] Cfg.domain_rand.tile_height_curriculum = False Cfg.domain_rand.tile_height_update_interval = 1000, 3000 Cfg.domain_rand.tile_height_curriculum_step = 0.01 Cfg.terrain.border_size = 0.0 Cfg.commands.resampling_time = 10 Cfg.reward_scales.feet_slip = -0.04 Cfg.reward_scales.action_smoothness_1 = -0.1 Cfg.reward_scales.action_smoothness_2 = -0.1 Cfg.reward_scales.dof_vel = -1e-4 Cfg.reward_scales.dof_pos = -0.05 Cfg.reward_scales.jump = 10.0 Cfg.reward_scales.base_height = 0.0 Cfg.rewards.base_height_target = 0.30 Cfg.reward_scales.estimation_bonus = 0.0 Cfg.reward_scales.feet_impact_vel = -0.0 # rewards.footswing_height = 0.09 Cfg.reward_scales.feet_clearance = -0.0 Cfg.reward_scales.feet_clearance_cmd = -15. # reward_scales.feet_contact_forces = -0.01 Cfg.rewards.reward_container_name = "CoRLRewards" Cfg.rewards.only_positive_rewards = False Cfg.rewards.only_positive_rewards_ji22_style = True Cfg.rewards.sigma_rew_neg = 0.02 Cfg.reward_scales.hop_symmetry = 0.0 Cfg.rewards.kappa_gait_probs = 0.07 Cfg.rewards.gait_force_sigma = 100. Cfg.rewards.gait_vel_sigma = 10. Cfg.reward_scales.tracking_contacts_shaped_force = 4.0 Cfg.reward_scales.tracking_contacts_shaped_vel = 4.0 Cfg.reward_scales.collision = -5.0 Cfg.commands.lin_vel_x = [-1.0, 1.0] Cfg.commands.lin_vel_y = [-0.6, 0.6] Cfg.commands.ang_vel_yaw = [-1.0, 1.0] Cfg.commands.body_height_cmd = [-0.25, 0.15] Cfg.commands.gait_frequency_cmd_range = [1.5, 4.0] Cfg.commands.gait_phase_cmd_range = [0.0, 1.0] Cfg.commands.gait_offset_cmd_range = [0.0, 1.0] Cfg.commands.gait_bound_cmd_range = [0.0, 1.0] Cfg.commands.gait_duration_cmd_range = [0.5, 0.5] Cfg.commands.footswing_height_range = [0.03, 0.25] Cfg.reward_scales.lin_vel_z = -0.02 Cfg.reward_scales.ang_vel_xy = -0.001 Cfg.reward_scales.base_height = 0.0 Cfg.reward_scales.feet_air_time = 0.0 Cfg.commands.limit_vel_x = [-5.0, 5.0] Cfg.commands.limit_vel_y = [-0.6, 0.6] Cfg.commands.limit_vel_yaw = [-5.0, 5.0] Cfg.commands.limit_body_height = [-0.25, 0.15] Cfg.commands.limit_gait_frequency = [1.5, 4.0] Cfg.commands.limit_gait_phase = [0.0, 1.0] Cfg.commands.limit_gait_offset = [0.0, 1.0] Cfg.commands.limit_gait_bound = [0.0, 1.0] Cfg.commands.limit_gait_duration = [0.5, 0.5] Cfg.commands.limit_footswing_height = [0.03, 0.25] Cfg.commands.num_bins_vel_x = 21 Cfg.commands.num_bins_vel_y = 1 Cfg.commands.num_bins_vel_yaw = 21 Cfg.commands.num_bins_body_height = 1 Cfg.commands.num_bins_gait_frequency = 1 Cfg.commands.num_bins_gait_phase = 1 Cfg.commands.num_bins_gait_offset = 1 Cfg.commands.num_bins_gait_bound = 1 Cfg.commands.num_bins_gait_duration = 1 Cfg.commands.num_bins_footswing_height = 1 Cfg.normalization.friction_range = [0, 1] Cfg.normalization.ground_friction_range = [0, 1] Cfg.commands.exclusive_phase_offset = False Cfg.commands.pacing_offset = False Cfg.commands.binary_phases = True Cfg.commands.gaitwise_curricula = True # 5 times per second Cfg.env.num_envs = 3 Cfg.domain_rand.push_interval_s = 1 Cfg.terrain.num_rows = 3 Cfg.terrain.num_cols = 5 Cfg.terrain.border_size = 0 Cfg.domain_rand.randomize_friction = True Cfg.domain_rand.friction_range = [1.0, 1.01] Cfg.domain_rand.randomize_base_mass = True Cfg.domain_rand.added_mass_range = [0., 6.] Cfg.terrain.terrain_noise_magnitude = 0.0 # Cfg.asset.fix_base_link = True Cfg.domain_rand.lag_timesteps = 6 Cfg.domain_rand.randomize_lag_timesteps = True Cfg.control.control_type = "P" env = VelocityTrackingEasyEnv(sim_device='cuda:0', headless=False, cfg=Cfg) env.reset() if render and headless: img = env.render(mode="rgb_array") plt.imshow(img) plt.show() print("Show the first frame and exit.") exit() for i in trange(1000, desc="Running"): actions = 0. * torch.ones(env.num_envs, env.num_actions, device=env.device) obs, rew, done, info = env.step(actions) # breakpoint() print("Done") if __name__ == '__main__': run_env(render=True, headless=False)