143 lines
4.6 KiB
Python
143 lines
4.6 KiB
Python
import sys
|
|
from legged_gym import LEGGED_GYM_ROOT_DIR
|
|
import os
|
|
import sys
|
|
from legged_gym import LEGGED_GYM_ROOT_DIR
|
|
|
|
import isaacgym
|
|
from legged_gym.envs import *
|
|
from legged_gym.utils import get_args, export_policy_as_jit, task_registry, Logger
|
|
|
|
import numpy as np
|
|
import torch
|
|
|
|
|
|
def play(args):
|
|
env_cfg, train_cfg = task_registry.get_cfgs(name=args.task)
|
|
# override some parameters for testing
|
|
env_cfg.env.num_envs = min(env_cfg.env.num_envs, 100)
|
|
env_cfg.terrain.num_rows = 5
|
|
env_cfg.terrain.num_cols = 5
|
|
env_cfg.terrain.curriculum = False
|
|
env_cfg.noise.add_noise = False
|
|
env_cfg.domain_rand.randomize_friction = False
|
|
env_cfg.domain_rand.push_robots = False
|
|
|
|
env_cfg.env.test = True
|
|
|
|
# prepare environment
|
|
env, _ = task_registry.make_env(name=args.task, args=args, env_cfg=env_cfg)
|
|
obs = env.get_observations()
|
|
# load policy
|
|
train_cfg.runner.resume = True
|
|
ppo_runner, train_cfg = task_registry.make_alg_runner(env=env, name=args.task, args=args, train_cfg=train_cfg)
|
|
policy = ppo_runner.get_inference_policy(device=env.device)
|
|
|
|
# Define the joint orders for sim A and sim B
|
|
sim_a_joints = [
|
|
'left_hip_pitch_joint',
|
|
'right_hip_pitch_joint',
|
|
'waist_yaw_joint',
|
|
'left_hip_roll_joint',
|
|
'right_hip_roll_joint',
|
|
'waist_roll_joint',
|
|
'left_hip_yaw_joint',
|
|
'right_hip_yaw_joint',
|
|
'waist_pitch_joint',
|
|
'left_knee_joint',
|
|
'right_knee_joint',
|
|
'left_shoulder_pitch_joint',
|
|
'right_shoulder_pitch_joint',
|
|
'left_ankle_pitch_joint',
|
|
'right_ankle_pitch_joint',
|
|
'left_shoulder_roll_joint',
|
|
'right_shoulder_roll_joint',
|
|
'left_ankle_roll_joint',
|
|
'right_ankle_roll_joint',
|
|
'left_shoulder_yaw_joint',
|
|
'right_shoulder_yaw_joint',
|
|
'left_elbow_joint',
|
|
'right_elbow_joint',
|
|
'left_wrist_roll_joint',
|
|
'right_wrist_roll_joint',
|
|
'left_wrist_pitch_joint',
|
|
'right_wrist_pitch_joint',
|
|
'left_wrist_yaw_joint',
|
|
'right_wrist_yaw_joint'
|
|
]
|
|
|
|
sim_b_joints = [
|
|
'left_hip_pitch_joint',
|
|
'left_hip_roll_joint',
|
|
'left_hip_yaw_joint',
|
|
'left_knee_joint',
|
|
'left_ankle_pitch_joint',
|
|
'left_ankle_roll_joint',
|
|
'right_hip_pitch_joint',
|
|
'right_hip_roll_joint',
|
|
'right_hip_yaw_joint',
|
|
'right_knee_joint',
|
|
'right_ankle_pitch_joint',
|
|
'right_ankle_roll_joint',
|
|
'waist_yaw_joint',
|
|
'waist_roll_joint',
|
|
'waist_pitch_joint',
|
|
'left_shoulder_pitch_joint',
|
|
'left_shoulder_roll_joint',
|
|
'left_shoulder_yaw_joint',
|
|
'left_elbow_joint',
|
|
'left_wrist_roll_joint',
|
|
'left_wrist_pitch_joint',
|
|
'left_wrist_yaw_joint',
|
|
'right_shoulder_pitch_joint',
|
|
'right_shoulder_roll_joint',
|
|
'right_shoulder_yaw_joint',
|
|
'right_elbow_joint',
|
|
'right_wrist_roll_joint',
|
|
'right_wrist_pitch_joint',
|
|
'right_wrist_yaw_joint'
|
|
]
|
|
|
|
# Create a mapping tensor
|
|
mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device)
|
|
|
|
# Fill the mapping tensor
|
|
for b_idx, b_joint in enumerate(sim_b_joints):
|
|
if b_joint in sim_a_joints:
|
|
a_idx = sim_a_joints.index(b_joint)
|
|
# mapping_tensor[b_idx, a_idx] = 1.0
|
|
mapping_tensor[a_idx, b_idx] = 1.0
|
|
# export policy as a jit module (used to run it from C++)
|
|
if EXPORT_POLICY:
|
|
path = os.path.join(LEGGED_GYM_ROOT_DIR, 'logs', train_cfg.runner.experiment_name, 'exported', 'policies')
|
|
export_policy_as_jit(ppo_runner.alg.actor_critic, path)
|
|
print('Exported policy as jit script to: ', path)
|
|
|
|
for i in range(10*int(env.max_episode_length)):
|
|
obs[..., 9:38] = obs[..., 9:38] @ mapping_tensor.transpose(0, 1)
|
|
obs[..., 38:67] = obs[..., 38:67] @ mapping_tensor.transpose(0, 1)
|
|
obs[..., 67:96] = obs[..., 67:96] @ mapping_tensor.transpose(0, 1)
|
|
# from icecream import ic
|
|
# ic(
|
|
# obs[..., :9],
|
|
# obs[..., 9:38],
|
|
# obs[..., 38:67],
|
|
# obs[..., 67:96],
|
|
|
|
# )
|
|
actions = policy(obs.detach())
|
|
# ic(
|
|
# actions
|
|
# )
|
|
reordered_actions = actions @ mapping_tensor
|
|
# obs, _, rews, dones, infos = env.step(actions.detach())
|
|
obs, _, rews, dones, infos = env.step(reordered_actions.detach())
|
|
|
|
if __name__ == '__main__':
|
|
EXPORT_POLICY = True
|
|
# EXPORT_POLICY = False
|
|
RECORD_FRAMES = False
|
|
MOVE_CAMERA = False
|
|
args = get_args()
|
|
play(args)
|