update gait

sim/humanoid_gym/envs/stompy_config.py

@@ -142,7 +142,7 @@ class ranges:
             lin_vel_x = [-0.3, 0.6]  # min max [m/s]
             lin_vel_y = [-0.3, 0.3]  # min max [m/s]
             ang_vel_yaw = [-0.3, 0.3]  # min max [rad/s]
-            heading = [-3.14, 3.14]
+            heading = [-.14, .14]
 
     class rewards:
         # quite important to keep it right

sim/humanoid_gym/envs/stompy_env.py

@@ -116,27 +116,29 @@ 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()
+        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
 
         # Double support phase
         self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0
 
         self.ref_action = 2 * self.ref_dof_pos
 
+
     def create_sim(self):
         """Creates simulation, terrain and evironments"""
         self.up_axis_idx = 2  # 2 for z, 1 for y -> adapt gravity accordingly

sim/scripts/create_fixed_torso.py

@@ -4,13 +4,13 @@
 import xml.etree.ElementTree as ET
 
 from sim.scripts.create_mjcf import create_mjcf
-from sim.stompy_legs.joints import Stompy
+from sim.stompy.joints import Stompy
 
-STOMPY_URDF = "sim/stompy_legs/robot.urdf"
+STOMPY_URDF = "sim/stompy"
 
 
 def update_urdf() -> None:
-    tree = ET.parse(STOMPY_URDF)
+    tree = ET.parse(STOMPY_URDF + "/robot.urdf")
     root = tree.getroot()
     stompy = Stompy()
     print(stompy.default_standing())
@@ -48,7 +48,7 @@ def update_urdf() -> None:
                         dynamics.set("friction", str(value))
 
     # Save the modified URDF to a new file
-    tree.write("sim/stompy_legs/robot_fixed.urdf")
+    tree.write(STOMPY_URDF + "/robot_fixed.urdf")
 
 
 if __name__ == "__main__":