EmbodiedAI
/
unitree_rl_gym
mirror of https://github.com/unitreerobotics/unitree_rl_gym.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

sim2sim implemented

This commit is contained in:
HYUNHONOH98 2025-02-17 16:43:33 +09:00
parent 22559fe14e
commit e49bc34d71
3 changed files with 293 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
deploy/deploy_mujoco/configs/g1_nav.yaml Normal file
View File

@ -0,0 +1,120 @@
#
# policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt"
# policy_path: "{LEGGED_GYM_ROOT_DIR}/logs/g1/walk_with_dr_test_v21/exported/policy.pt"
policy_path: "{LEGGED_GYM_ROOT_DIR}/policies/2025-02-16_22-42-31/policy.pt"
# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/scene.xml"
xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_29dof_rev_1_0.xml"
lab_joint: [
'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'
]
motor_joint: [
'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'
]
# Total simulation time
simulation_duration: 60.0
# simulation_duration: 5.
# Simulation time step
simulation_dt: 0.002
# Controller update frequency (meets the requirement of simulation_dt * controll_decimation=0.02; 50Hz)
control_decimation: 10
# kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40]
kps: [
100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40,
150, 150, 150,
100, 100, 50, 50, 20, 20, 20,
100, 100, 50, 50, 20, 20, 20
]
kds: [
2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2,
3, 3, 3,
2, 2, 2, 2, 1, 1, 1,
2, 2, 2, 2, 1, 1, 1
]
default_angles: [
-0.2, 0.0, 0.0, 0.42, -0.23, 0.0,
-0.2, 0.0, 0.0, 0.42, -0.23, 0.0,
0., 0., 0.,
0.35, 0.16, 0., 0.87, 0., 0., 0.,
0.35, -0.16, 0., 0.87, 0., 0., 0.,
]
# ang_vel_scale: 0.25
ang_vel_scale: 1.0
dof_pos_scale: 1.0
# dof_vel_scale: 0.05
dof_vel_scale: 1.0
# action_scale: 0.25
# action_scale: 1.0
action_scale: 0.4
# cmd_scale: [2.0, 2.0, 0.25]
cmd_scale: [1.0, 1.0, 1.0, 1.0]
# cmd_scale: [0., 0., 0.]
# num_actions: 12
num_actions: 29
# num_obs: 47
# num_obs: 96
num_obs: 97
cmd_init: [3, 0, 0, 0]

172
deploy/deploy_mujoco/deploy_mujoco_navigation.py Normal file
View File

@ -0,0 +1,172 @@
import time
import mujoco.viewer
import mujoco
import numpy as np
from legged_gym import LEGGED_GYM_ROOT_DIR
import torch
import yaml
from icecream import ic
import deploy.deploy_real.math_utils as math_utils
def get_gravity_orientation(quaternion):
qw = quaternion[0]
qx = quaternion[1]
qy = quaternion[2]
qz = quaternion[3]
gravity_orientation = np.zeros(3)
gravity_orientation[0] = 2 * (-qz * qx + qw * qy)
gravity_orientation[1] = -2 * (qz * qy + qw * qx)
gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz)
return gravity_orientation
def pd_control(target_q, q, kp, target_dq, dq, kd):
"""Calculates torques from position commands"""
return (target_q - q) * kp + (target_dq - dq) * kd
if __name__ == "__main__":
# get config file name from command line
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("config_file", type=str, help="config file name in the config folder")
args = parser.parse_args()
config_file = args.config_file
with open(f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_mujoco/configs/{config_file}", "r") as f:
config = yaml.load(f, Loader=yaml.FullLoader)
isaaclab_joint_order = config["lab_joint"]
raw_joint_order = config["motor_joint"]
mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order)))
# Fill the mapping tensor
for b_idx, b_joint in enumerate(raw_joint_order):
if b_joint in isaaclab_joint_order:
a_idx = isaaclab_joint_order.index(b_joint)
mapping_tensor[a_idx, b_idx] = 1.0
policy_path = config["policy_path"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR)
xml_path = config["xml_path"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR)
simulation_duration = config["simulation_duration"]
simulation_dt = config["simulation_dt"]
control_decimation = config["control_decimation"]
kps = np.array(config["kps"], dtype=np.float32)
kds = np.array(config["kds"], dtype=np.float32)
default_angles = np.array(config["default_angles"], dtype=np.float32)
ang_vel_scale = config["ang_vel_scale"]
dof_pos_scale = config["dof_pos_scale"]
dof_vel_scale = config["dof_vel_scale"]
action_scale = config["action_scale"]
cmd_scale = np.array(config["cmd_scale"], dtype=np.float32)
num_actions = config["num_actions"]
num_obs = config["num_obs"]
cmd = np.array(config["cmd_init"], dtype=np.float32)
target_cmd = np.array(config["cmd_init"], dtype=np.float32)
# define context variables
action = np.zeros(num_actions, dtype=np.float32)
target_dof_pos = default_angles.copy()
obs = np.zeros(num_obs, dtype=np.float32)
counter = 0
# Load robot model
m = mujoco.MjModel.from_xml_path(xml_path)
d = mujoco.MjData(m)
m.opt.timestep = simulation_dt
# load policy
policy = torch.jit.load(policy_path)
with mujoco.viewer.launch_passive(m, d) as viewer:
# Close the viewer automatically after simulation_duration wall-seconds.
start = time.time()
while viewer.is_running() and time.time() - start < simulation_duration:
step_start = time.time()
tau = pd_control(target_dof_pos, d.qpos[7:], kps, np.zeros_like(kds), d.qvel[6:], kds)
d.ctrl[:] = tau
# mj_step can be replaced with code that also evaluates
# a policy and applies a control signal before stepping the physics.
mujoco.mj_step(m, d)
counter += 1
if counter % control_decimation == 0:
# create observation
qj = d.qpos[7:]
dqj = d.qvel[6:]
quat = d.qpos[3:7]
omega = d.qvel[3:6]
position = d.qpos[:3]
# position is given on world frame.
# ic(position)
ic(cmd)
cmd[0] = target_cmd[0] - position[0]
cmd[1] = target_cmd[1] - position[1]
# cmd[2] -= position[2]
cmd[:3] = math_utils.quat_rotate_inverse(
math_utils.yaw_quat(torch.as_tensor(quat).float().to("cpu")),
torch.as_tensor(cmd[:3]).float().to("cpu")
).float().numpy()
qj = (qj - default_angles) * dof_pos_scale
dqj = dqj * dof_vel_scale
gravity_orientation = get_gravity_orientation(quat)
omega = omega * ang_vel_scale
period = 0.8
count = counter * simulation_dt
phase = count % period / period
obs[:3] = omega
obs[3:6] = gravity_orientation
obs[6:10] = cmd * cmd_scale
obs[10 : 10 + num_actions] = qj
obs[10 + num_actions : 10 + 2 * num_actions] = dqj
obs[10 + 2 * num_actions : 10 + 3 * num_actions] = action
# obs[9 + 3 * num_actions : 9 + 3 * num_actions + 2] = np.array([sin_phase, cos_phase])
obs_tensor = torch.from_numpy(obs).unsqueeze(0)
obs_tensor[..., 10:39] = obs_tensor[..., 10:39] @ mapping_tensor.transpose(0, 1)
obs_tensor[..., 39:68] = obs_tensor[..., 39:68] @ mapping_tensor.transpose(0, 1)
obs_tensor[..., 68:97] = obs_tensor[..., 68:97] @ mapping_tensor.transpose(0, 1)
# policy inference
action = policy(obs_tensor).detach().numpy().squeeze()
# reordered_actions = action @ mapping_tensor.detach().cpu().numpy()
# applied_action = action @ mapping_tensor.detach().cpu().numpy()
action = action @ mapping_tensor.detach().cpu().numpy()
# ic(
# action
# )
# action = 0.
# transform action to target_dof_pos
# target_dof_pos = action * action_scale + default_angles
target_dof_pos = action * action_scale + default_angles
# raise NotImplementedError
# Pick up changes to the physics state, apply perturbations, update options from GUI.
viewer.sync()
# Rudimentary time keeping, will drift relative to wall clock.
time_until_next_step = m.opt.timestep - (time.time() - step_start)
if time_until_next_step > 0:
time.sleep(time_until_next_step)

2
deploy/deploy_real/deploy_real_ros_navigation.py
View File

@ -337,7 +337,7 @@ class Controller:
action = np.zeros_like(self.action,
dtype=np.float32)
action[self.mot_from_lab] = self.action
action[self.mot_from_lab] = self.action # index_map works ?
target_dof_pos = self.config.default_angles + action * self.config.action_scale