working commit

sim/deploy/config.py

@@ -58,7 +58,7 @@ class RobotConfig:
     tau_limit: np.ndarray
     robot_model_path: str
     # is2ac
-    dt: float = 0.00002  # 0.001
+    dt: float = 0.0002  # 0.001
     phase_duration: float = 0.64
     duration: float = 10.0
     decimation: int = 10

sim/deploy/policy.py

@@ -109,7 +109,10 @@ def pd_control(
             Calculated torques.
         """
         target_dof_vel = np.zeros(self.cfg.num_actions, dtype=np.double)
-        return (target_dof_pos - dof_pos) * kp + (target_dof_vel - dof_vel) * kd
+        # breakpoint()
+        res = ( (target_dof_pos - dof_pos) * kp + (target_dof_vel - dof_vel) * kd )
+        # breakpoint()
+        return res * 0.75
 
     def parse_action(
         self, dof_pos: np.ndarray, dof_vel: np.ndarray, eu_ang: np.ndarray, ang_vel: np.ndarray, count_lowlevel: int

sim/deploy/run.py

@@ -22,7 +22,7 @@
 
 from sim.deploy.config import RobotConfig
 from sim.env import stompy_mjcf_path
-from sim.new_test.joints import Stompy as StompyFixed
+from sim.stompy_legs.joints import Stompy as StompyFixed
 
 
 class Worlds(Enum):
@@ -88,7 +88,7 @@ def step(
             dq: The current joint velocities.
             target_dq: The target joint velocities (optional).
         """
-        tau = np.clip(tau, -self.cfg.tau_limit, self.cfg.tau_limit)
+        # tau = np.clip(tau, -self.cfg.tau_limit, self.cfg.tau_limit)
         self.data.ctrl = tau
         mujoco.mj_step(self.model, self.data)
 
@@ -125,14 +125,15 @@ def simulate(self, policy: SimPolicy) -> None:
                 dof_pos, dof_vel, orientation, ang_vel = self.get_observation()
 
                 # Zero action
-                target_dof_pos = dof_pos
+                # target_dof_pos = dof_pos
 
                 # We update the policy at a lower frequency
                 # The simulation runs at 1000hz, but the policy is updated at 100hz
-                # if step % cfg.decimation == 0:
-                #     action = policy.next_action(dof_pos, dof_vel, orientation, ang_vel, step)
-                #     target_dof_pos = action * cfg.action_scale
-
+                if step % cfg.decimation == 0:
+                    action = policy.next_action(dof_pos, dof_vel, orientation, ang_vel, step)
+                    target_dof_pos = action
+                # set target_dof_pos to 0s
+                # target_dof_pos = np.zeros_like(target_dof_pos)
                 tau = policy.pd_control(target_dof_pos, dof_pos, cfg.kps, dof_vel, cfg.kds)
                 # breakpoint()
                 # set tau to zero for now
@@ -271,9 +272,13 @@ def main(args: argparse.Namespace, cfg: RobotConfig) -> None:
     robot_path = stompy_mjcf_path(legs_only=True)
     num_single_obs = dof * 3 + 11
 
-    kps = np.ones((dof), dtype=np.double) * 200
-    kds = np.ones((dof), dtype=np.double) * 10
-    tau_limit = np.ones((dof), dtype=np.double) * 200
+    # is2ac, lets scale kps and kds to be the same as our stiffness and dmaping
+    kps = np.ones((dof), dtype=np.double) * 0.00001 * 25
+    kds = np.ones((dof), dtype=np.double) * 1
+    tau_limit = np.ones((dof), dtype=np.double) * 0.00001 * 25
+    # kps = np.ones((dof), dtype=np.double) * 200
+    # kds = np.ones((dof), dtype=np.double) * 10
+    # tau_limit = np.ones((dof), dtype=np.double) * 200
 
     cfg = RobotConfig(
         robot_model_path=robot_path, dof=dof, kps=kps, kds=kds, tau_limit=tau_limit, num_single_obs=num_single_obs

sim/env.py

@@ -9,10 +9,7 @@
 
 
 def model_dir() -> Path:
-    # return Path(os.environ.get("MODEL_DIR", "models"))
-    # return Path(os.environ.get("MODEL_DIR", "stompytherobot"))
-    # return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2")
-    return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2")
+    return Path(os.environ.get("MODEL_DIR", "models"))
 
 
 def run_dir() -> Path:

sim/humanoid_gym/envs/__init__.py

@@ -13,8 +13,6 @@
 # fmt: on
 from .getup_config import GetupCfg, GetupCfgPPO
 from .getup_env import GetupFreeEnv
-from .hexmove_config import HexmoveCfg, HexmoveCfgPPO
-from .hexmove_env import HexmoveFreeEnv
 from .legs_config import LegsCfg, LegsCfgPPO
 from .legs_env import LegsFreeEnv
 from .only_legs_config import OnlyLegsCfg, OnlyLegsCfgPPO

sim/humanoid_gym/envs/only_legs_config.py

@@ -6,7 +6,7 @@
 )
 
 from sim.env import stompy_urdf_path
-from sim.new_test.joints import Stompy
+from sim.stompy_legs.joints import Stompy
 
 NUM_JOINTS = len(Stompy.all_joints())  # 33
 
@@ -149,12 +149,12 @@ class domain_rand:
 
         randomize_base_mass = True
         # added_mass_range = [-1.0, 1.0]
-        added_mass_range = [-0.2, 0.2]
+        added_mass_range = [-0.5, 0.5]
         push_robots = True
         push_interval_s = 4
         max_push_vel_xy = 0.2
         max_push_ang_vel = 0.4
-        dynamic_randomization = 0.02
+        dynamic_randomization = 0.05
 
     class commands(LeggedRobotCfg.commands):
         # Vers: lin_vel_x, lin_vel_y, ang_vel_yaw, heading (in heading mode ang_vel_yaw is recomputed from heading error)
@@ -171,8 +171,13 @@ class ranges:
     class rewards:
         # quite important to keep it right
         base_height_target = 0.72
+
+        #distance between the knees and feet is2ac
         min_dist = 0.2
         max_dist = 0.5
+        # min_dist = 0.4
+        # max_dist = 0.7
+
         # put some settings here for LLM parameter tuning
         target_joint_pos_scale = 0.17  # rad
         target_feet_height = 0.06  # m
@@ -184,7 +189,7 @@ class rewards:
         max_contact_force = 400  # forces above this value are penalized
 
         class scales:
-            # # reference motion tracking
+            # reference motion tracking
             # joint_pos = 1.6
             # feet_clearance = 1.0
             # feet_contact_number = 1.2

sim/humanoid_gym/envs/only_legs_env.py

@@ -298,12 +298,12 @@ def _reward_knee_distance(self):
         """
         Calculates the reward based on the distance between the knee of the humanoid.
         """
-        foot_pos = self.rigid_state[:, self.knee_indices, :2]
-        foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1)
+        knee_pos = self.rigid_state[:, self.knee_indices, :2]
+        knee_dist = torch.norm(knee_pos[:, 0, :] - knee_pos[:, 1, :], dim=1)
         fd = self.cfg.rewards.min_dist
         max_df = self.cfg.rewards.max_dist / 2
-        d_min = torch.clamp(foot_dist - fd, -0.5, 0.0)
-        d_max = torch.clamp(foot_dist - max_df, 0, 0.5)
+        d_min = torch.clamp(knee_dist - fd, -0.5, 0.0)
+        d_max = torch.clamp(knee_dist - max_df, 0, 0.5)
         return (torch.exp(-torch.abs(d_min) * 100) + torch.exp(-torch.abs(d_max) * 100)) / 2
 
     def _reward_foot_slip(self):

sim/stompy_legs/joints.py

@@ -85,60 +85,49 @@ def default_positions(cls) -> Dict[str, float]:
     @classmethod
     def default_standing(cls) -> Dict[str, float]:
         return {
-            # # # legs
-            Stompy.legs.left.hip_pitch: 1.61,
-            Stompy.legs.left.hip_roll: 0,
-            Stompy.legs.left.hip_yaw: 1,
-            Stompy.legs.left.knee_pitch: 2.05,
-            Stompy.legs.left.ankle_pitch: 0.33,
-            Stompy.legs.left.ankle_roll: 1.73,
+            # legs
             Stompy.legs.right.hip_pitch: 0,
-            Stompy.legs.right.hip_roll: -1.6,
             Stompy.legs.right.hip_yaw: -2.15,
+            Stompy.legs.right.hip_roll: -1.6,
             Stompy.legs.right.knee_pitch: 2.16,
             Stompy.legs.right.ankle_pitch: 0.5,
             Stompy.legs.right.ankle_roll: 1.72,
+            Stompy.legs.left.hip_pitch: 1.61,
+            Stompy.legs.left.hip_yaw: 1,
+            Stompy.legs.left.hip_roll: 0,
+            Stompy.legs.left.knee_pitch: 2.05,
+            Stompy.legs.left.ankle_pitch: 0.33,
+            Stompy.legs.left.ankle_roll: 1.73,
+            # legs in squat
+            # Stompy.legs.left.hip_pitch: 1.17,
+            # Stompy.legs.left.hip_roll: 0.1,
+            # Stompy.legs.left.hip_yaw: 1.03,
+            # Stompy.legs.left.knee_pitch: 1.13,
+            # Stompy.legs.left.ankle_pitch: -0.27,
+            # Stompy.legs.left.ankle_roll: 1.73,
+            # Stompy.legs.right.hip_pitch: 0.38,
+            # Stompy.legs.right.hip_roll: -1.6,
+            # Stompy.legs.right.hip_yaw: -2.15,
+            # Stompy.legs.right.knee_pitch: 2.94,
+            # Stompy.legs.right.ankle_pitch: 0.8,
+            # Stompy.legs.right.ankle_roll: 1.72,
         }
 
     @classmethod
     def default_limits(cls) -> Dict[str, Dict[str, float]]:
         return {
-            Stompy.legs.left.hip_pitch: {
-                "lower": 0.5,
-                "upper": 2.69,
-            },
-            Stompy.legs.left.hip_roll: {
-                "lower": -0.5,
-                "upper": 0.5,
-            },
-            Stompy.legs.left.hip_yaw: {
-                "lower": 0.5,
-                "upper": 1.19,
-            },
-            Stompy.legs.left.knee_pitch: {
-                "lower": 1,
-                "upper": 3.1,
-            },
-            Stompy.legs.left.ankle_pitch: {
-                "lower": -0.3,
-                "upper": 1.13,
-            },
-            Stompy.legs.left.ankle_roll: {
-                "lower": 1.3,
-                "upper": 2,
-            },
             Stompy.legs.right.hip_pitch: {
                 "lower": -1,
                 "upper": 1,
             },
-            Stompy.legs.right.hip_roll: {
-                "lower": -2.39,
-                "upper": -1,
-            },
             Stompy.legs.right.hip_yaw: {
                 "lower": -2.2,
                 "upper": -1,
             },
+            Stompy.legs.right.hip_roll: {
+                "lower": -2.39,
+                "upper": -1,
+            },
             Stompy.legs.right.knee_pitch: {
                 "lower": 1.54,
                 "upper": 3,
@@ -151,6 +140,30 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
                 "lower": 1,
                 "upper": 2.3,
             },
+            Stompy.legs.left.hip_pitch: {
+                "lower": 0.5,
+                "upper": 2.69,
+            },
+            Stompy.legs.left.hip_yaw: {
+                "lower": 0.5,
+                "upper": 1.19,
+            },
+            Stompy.legs.left.hip_roll: {
+                "lower": -0.5,
+                "upper": 0.5,
+            },
+            Stompy.legs.left.knee_pitch: {
+                "lower": 1,
+                "upper": 3.1,
+            },
+            Stompy.legs.left.ankle_pitch: {
+                "lower": -0.3,
+                "upper": 1.13,
+            },
+            Stompy.legs.left.ankle_roll: {
+                "lower": 1.3,
+                "upper": 2,
+            },
         }