206 lines
7.5 KiB
Python
206 lines
7.5 KiB
Python
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) |