Navigation sim2sim, sim2real

MUJOCO_LOG.TXT

@@ -0,0 +1,6 @@
+Sat Jan 25 16:14:06 2025
+WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.1650.
+
+Sat Jan 25 16:23:57 2025
+WARNING: Nan, Inf or huge value in QACC at DOF 18. The simulation is unstable. Time = 0.1150.
+

deploy/deploy_mujoco/configs/g1.yaml

@@ -1,26 +1,54 @@
 # 
-policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt"
-xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/scene.xml"
+# 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}/logs/g1/walk_with_dr_test_v22/exported/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"
 
 # 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]
-kds: [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2]
+# 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.1,  0.0,  0.0,  0.3, -0.2, 0.0, 
-                  -0.1,  0.0,  0.0,  0.3, -0.2, 0.0]
+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: 0.25
+ang_vel_scale: 1.0
 dof_pos_scale: 1.0
-dof_vel_scale: 0.05
-action_scale: 0.25
-cmd_scale: [2.0, 2.0, 0.25]
-num_actions: 12
-num_obs: 47
+# dof_vel_scale: 0.05
+dof_vel_scale: 1.0
+# action_scale: 0.25
+# action_scale: 1.0
+action_scale: 0.5
+# cmd_scale: [2.0, 2.0, 0.25]
+cmd_scale: [1.0, 1.0, 1.0]
+# cmd_scale: [0., 0., 0.]
+# num_actions: 12
+num_actions: 29
+# num_obs: 47
+num_obs: 96
 
 cmd_init: [0.5, 0, 0]

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/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"
+
+
+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]

deploy/deploy_mujoco/deploy_mujoco.py

@@ -30,6 +30,81 @@ def pd_control(target_q, q, kp, target_dq, dq, kd):
 
 if __name__ == "__main__":
     # get config file name from command line
+    isaaclab_joint_order = [
+        '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'
+    ]
+
+    raw_joint_order = [
+        '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)
+    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[b_idx, a_idx] = 1.0
+            mapping_tensor[a_idx, b_idx] = 1.0
+
     import argparse
 
     parser = argparse.ArgumentParser()
@@ -81,6 +156,10 @@ def pd_control(target_q, q, kp, target_dq, dq, kd):
         start = time.time()
         while viewer.is_running() and time.time() - start < simulation_duration:
             step_start = time.time()
+            from icecream import ic
+            # ic(
+            #     target_dof_pos, d.qpos[7:], kps, np.zeros_like(kds), d.qvel[6:], kds
+            # )
             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
@@ -114,12 +193,32 @@ def pd_control(target_q, q, kp, target_dq, dq, kd):
                 obs[9 : 9 + num_actions] = qj
                 obs[9 + num_actions : 9 + 2 * num_actions] = dqj
                 obs[9 + 2 * num_actions : 9 + 3 * num_actions] = action
-                obs[9 + 3 * num_actions : 9 + 3 * num_actions + 2] = np.array([sin_phase, cos_phase])
+                # 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[..., 9:38] = obs_tensor[..., 9:38] @ mapping_tensor.transpose(0, 1)
+                obs_tensor[..., 38:67] = obs_tensor[..., 38:67] @ mapping_tensor.transpose(0, 1)
+                obs_tensor[..., 67:96] = obs_tensor[..., 67:96] @ mapping_tensor.transpose(0, 1)
+                # from icecream import ic
+                # ic(
+
+                #     obs[..., :9],
+                #     obs[..., 9:38],
+                #     obs[..., 38:67],
+                #     obs[..., 67:96],
+                # )
                 # policy inference
                 action = policy(obs_tensor).detach().numpy().squeeze()
+                # reordered_actions = 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()