From 91344163ea2bbfe05b784b713a0678e5e2753501 Mon Sep 17 00:00:00 2001 From: HYUNHONOH98 Date: Fri, 21 Feb 2025 12:59:46 +0900 Subject: [PATCH] update mujoco sim2sim - heading direction command added --- deploy/deploy_mujoco/configs/g1_nav.yaml | 2 +- deploy/deploy_mujoco/deploy_mujoco_navigation.py | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/deploy/deploy_mujoco/configs/g1_nav.yaml b/deploy/deploy_mujoco/configs/g1_nav.yaml index 636d0e2..0c2cf8e 100644 --- a/deploy/deploy_mujoco/configs/g1_nav.yaml +++ b/deploy/deploy_mujoco/configs/g1_nav.yaml @@ -1,7 +1,7 @@ # # 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" +policy_path: "{LEGGED_GYM_ROOT_DIR}/policies/navigation_less_obs/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" diff --git a/deploy/deploy_mujoco/deploy_mujoco_navigation.py b/deploy/deploy_mujoco/deploy_mujoco_navigation.py index a920986..4c255a8 100644 --- a/deploy/deploy_mujoco/deploy_mujoco_navigation.py +++ b/deploy/deploy_mujoco/deploy_mujoco_navigation.py @@ -127,6 +127,12 @@ if __name__ == "__main__": torch.as_tensor(cmd[:3]).float().to("cpu") ).float().numpy() + robot_forward_vec_w = math_utils.quat_apply(torch.as_tensor(quat).float().to("cpu"), torch.tensor([1,0,0]).float().to("cpu")) + robot_heading_w = torch.atan2(robot_forward_vec_w[1], robot_forward_vec_w[0]) + cmd[3] = math_utils.wrap_to_pi(cmd[3] - robot_heading_w) + + ic(cmd) + qj = (qj - default_angles) * dof_pos_scale dqj = dqj * dof_vel_scale