From e49bc34d71b140362cb7e87cdbbbf5f9738cb247 Mon Sep 17 00:00:00 2001 From: HYUNHONOH98 Date: Mon, 17 Feb 2025 16:43:33 +0900 Subject: [PATCH] sim2sim implemented --- deploy/deploy_mujoco/configs/g1_nav.yaml | 120 ++++++++++++ .../deploy_mujoco/deploy_mujoco_navigation.py | 172 ++++++++++++++++++ .../deploy_real/deploy_real_ros_navigation.py | 2 +- 3 files changed, 293 insertions(+), 1 deletion(-) create mode 100644 deploy/deploy_mujoco/configs/g1_nav.yaml create mode 100644 deploy/deploy_mujoco/deploy_mujoco_navigation.py diff --git a/deploy/deploy_mujoco/configs/g1_nav.yaml b/deploy/deploy_mujoco/configs/g1_nav.yaml new file mode 100644 index 0000000..636d0e2 --- /dev/null +++ b/deploy/deploy_mujoco/configs/g1_nav.yaml @@ -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] \ No newline at end of file diff --git a/deploy/deploy_mujoco/deploy_mujoco_navigation.py b/deploy/deploy_mujoco/deploy_mujoco_navigation.py new file mode 100644 index 0000000..55ba4ac --- /dev/null +++ b/deploy/deploy_mujoco/deploy_mujoco_navigation.py @@ -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) diff --git a/deploy/deploy_real/deploy_real_ros_navigation.py b/deploy/deploy_real/deploy_real_ros_navigation.py index 8a2b4e2..563b043 100644 --- a/deploy/deploy_real/deploy_real_ros_navigation.py +++ b/deploy/deploy_real/deploy_real_ros_navigation.py @@ -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