From c9b361225239f8f647b7dbca4ba90ec620965ece Mon Sep 17 00:00:00 2001
From: is2ac2 <wlight@emory.edu>
Date: Thu, 18 Jul 2024 16:11:44 -0700
Subject: [PATCH] fixing gait progress

---
 sim/humanoid_gym/envs/only_legs_config.py |  2 +-
 sim/humanoid_gym/envs/only_legs_env.py    | 20 +++++++++++++-------
 sim/humanoid_gym/play.py                  |  3 ++-
 3 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/sim/humanoid_gym/envs/only_legs_config.py b/sim/humanoid_gym/envs/only_legs_config.py
index 9ebf5ce9..374b4e6d 100755
--- a/sim/humanoid_gym/envs/only_legs_config.py
+++ b/sim/humanoid_gym/envs/only_legs_config.py
@@ -75,7 +75,7 @@ class terrain(LeggedRobotCfg.terrain):
         restitution = 0.0
 
     class noise:
-        add_noise = False
+        add_noise = True
         noise_level = 0.6  # scales other values
 
         class noise_scales:
diff --git a/sim/humanoid_gym/envs/only_legs_env.py b/sim/humanoid_gym/envs/only_legs_env.py
index 3caef476..95e7d668 100755
--- a/sim/humanoid_gym/envs/only_legs_env.py
+++ b/sim/humanoid_gym/envs/only_legs_env.py
@@ -115,21 +115,27 @@ def compute_ref_state(self):
         sin_pos = torch.sin(2 * torch.pi * phase)
         sin_pos_l = sin_pos.clone()
         sin_pos_r = sin_pos.clone()
+        # breakpoint()
+        # self.ref_dof_pos = torch.zeros_like(self.dof_pos)
+        default_clone = self.default_dof_pos.clone()
+        self.ref_dof_pos = self.default_dof_pos.repeat(self.num_envs, 1)
 
-        self.ref_dof_pos = torch.zeros_like(self.dof_pos)
         scale_1 = self.cfg.rewards.target_joint_pos_scale
         scale_2 = 2 * scale_1
+
         # left foot stance phase set to default joint pos
         sin_pos_l[sin_pos_l > 0] = 0
-        self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] = sin_pos_l * scale_1
-        self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] = sin_pos_l * scale_2
-        self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] = sin_pos_l * scale_1
+        self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] += sin_pos_l * scale_1
+        self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] += sin_pos_l * scale_2
+        self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] += sin_pos_l * scale_1
 
         # right foot stance phase set to default joint pos
         sin_pos_r[sin_pos_r < 0] = 0
-        self.ref_dof_pos[:, self.legs_joints["right_hip_pitch"]] = sin_pos_r * scale_1
-        self.ref_dof_pos[:, self.legs_joints["right_knee_pitch"]] = sin_pos_r * scale_2
-        self.ref_dof_pos[:, self.legs_joints["right_ankle_roll"]] = sin_pos_r * scale_1
+        self.ref_dof_pos[:, self.legs_joints["right_hip_pitch"]] += sin_pos_r * scale_1
+        self.ref_dof_pos[:, self.legs_joints["right_knee_pitch"]] += sin_pos_r * scale_2
+        self.ref_dof_pos[:, self.legs_joints["right_ankle_roll"]] += sin_pos_r * scale_1
+
+        # breakpoint()
 
         # Double support phase
         self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0
diff --git a/sim/humanoid_gym/play.py b/sim/humanoid_gym/play.py
index e760f5de..1edeab0a 100755
--- a/sim/humanoid_gym/play.py
+++ b/sim/humanoid_gym/play.py
@@ -36,6 +36,7 @@ def play(args: argparse.Namespace) -> None:
     env_cfg.domain_rand.joint_angle_noise = 0.0
     env_cfg.noise.curriculum = False
     env_cfg.noise.noise_level = 0.5
+    env_cfg.sim.physx.solver_type = 0
 
     train_cfg.seed = 123145
     logger.info("train_cfg.runner_class_name: %s", train_cfg.runner_class_name)
@@ -90,7 +91,7 @@ def play(args: argparse.Namespace) -> None:
 
         if FIX_COMMAND:
             env.commands[:, 0] = 0.0
-            env.commands[:, 1] = -0.5  # negative left, positive right
+            env.commands[:, 1] = -0.2  # negative left, positive right
             env.commands[:, 2] = 0.0
             env.commands[:, 3] = 0.0