add stompy v2 (#20)

* added stompyv2

* stompy_v2 fixes

* removing torques values the model doesn't fly away

* updated standing position and joint limits

* initial params

* cleanup

* fighting mypy

* linting

* defeating mypy

* fixes

* final fixes

* more fixes

* more fixes

* linting

* fix linting

* fix pasta

---------

Co-authored-by: nathanjzhao <nathanzh@stanford.edu>
Co-authored-by: budzianowski <budzianowski@gmail.com>

.gitignore

@@ -1,7 +1,11 @@
 # .gitignore
-
+# Robot Parts
+*.stl
+*.dae
+*.urdf
 # Local files
 .env
+*.DS_Store
 # Python
 *.py[oc]
 __pycache__/
@@ -12,7 +16,6 @@ __pycache__/
 .pytest_cache/
 .ruff_cache/
 .dmypy.json
-
 # Databases
 *.db
 

README.md

@@ -80,6 +80,7 @@ python sim/humanoid_gym/train.py --task=stompy_ppo --num_envs=4096 --headless
 3. Run evaluation with the following command:
 ```bash
 python sim/humanoid_gym/play.py --task legs_ppo --sim_device cpu
+
 ```
 See [this doc](https://docs.google.com/document/d/1YZzBqIXO7oq7vIKu-BZr5ssNsi3nKtxpRPnfSnTXojA/edit?usp=sharing) for more beginner tips.
 

sim/humanoid_gym/envs/__init__.py

@@ -7,9 +7,10 @@
 quickly.
 """
 
-import isaacgym
-import torch
-
+# fmt: off
+import isaacgym # isort:skip
+import torch #isort:skip
+# fmt: on
 from .getup_config import GetupCfg, GetupCfgPPO
 from .getup_env import GetupFreeEnv
 from .legs_config import LegsCfg, LegsCfgPPO

sim/humanoid_gym/envs/getup_config.py

@@ -39,8 +39,9 @@ class asset(LeggedRobotCfg.asset):  # noqa: N801
         file = str(stompy_urdf_path())
 
         name = "stompy"
-        foot_name = "_leg_1_x4_1_outer_1"  # "foot"
-        knee_name = "belt_knee"  # "knee"
+        foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1"
+        knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1"
+
         terminate_after_contacts_on = []  # "link_torso_1_top_torso_1"]
         penalize_contacts_on = []
         self_collisions = 1  # 1 to disable, 0 to enable...bitwise filter

sim/humanoid_gym/envs/legs_config.py

@@ -33,13 +33,13 @@ class asset(LeggedRobotCfg.asset):
 
         name = "stompy"
 
-        foot_name = "_leg_1_x4_1_outer_1"  # "foot"
-        knee_name = "belt_knee"
+        foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1"
+        knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1"
 
         termination_height = 0.23
         default_feet_height = 0.0
 
-        terminate_after_contacts_on = ["link_torso_1_top_torso_1"]
+        terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"]
 
         penalize_contacts_on = []
         self_collisions = 1  # 1 to disable, 0 to enable...bitwise filter

sim/humanoid_gym/envs/legs_env.py

@@ -1,7 +1,7 @@
 # mypy: disable-error-code="valid-newtype"
 """Defines the environment for training the Stompy with fixed torso."""
 
-import torch
+import torch  # type: ignore[import]
 from humanoid.envs import LeggedRobot
 from humanoid.envs.base.legged_robot_config import LeggedRobotCfg
 from humanoid.utils.terrain import HumanoidTerrain
@@ -124,14 +124,14 @@ def compute_ref_state(self):
         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"]] = sin_pos_l * scale_2
-        self.ref_dof_pos[:, self.legs_joints["left_ankle"]] = 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"]] = sin_pos_r * scale_2
-        self.ref_dof_pos[:, self.legs_joints["right_ankle"]] = 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

sim/humanoid_gym/envs/stompy_config.py

@@ -1,11 +1,14 @@
 """Defines the environment configuration for the Getting up task"""
 
-from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
+from humanoid.envs.base.legged_robot_config import (  # type: ignore
+    LeggedRobotCfg,
+    LeggedRobotCfgPPO,
+)
 
 from sim.env import stompy_urdf_path
 from sim.stompy.joints import Stompy
 
-NUM_JOINTS = len(Stompy.all_joints())  # 37
+NUM_JOINTS = len(Stompy.all_joints())  # 33
 
 
 class StompyCfg(LeggedRobotCfg):
@@ -38,13 +41,12 @@ class asset(LeggedRobotCfg.asset):
 
         name = "stompy"
 
-        foot_name = "_leg_1_x4_1_outer_1"  # "foot"
-        knee_name = "belt_knee"
+        foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1"
+        knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1"
 
         termination_height = 0.23
         default_feet_height = 0.0
-
-        terminate_after_contacts_on = ["link_torso_1_top_torso_1"]
+        terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"]
 
         penalize_contacts_on = []
         self_collisions = 1  # 1 to disable, 0 to enable...bitwise filter
@@ -82,7 +84,7 @@ class noise_scales:
             height_measurements = 0.1
 
     class init_state(LeggedRobotCfg.init_state):
-        pos = [0.0, 0.0, 1.05]
+        pos = [0.0, 0.0, 1.15]
 
         default_joint_angles = {k: 0.0 for k in Stompy.all_joints()}
 
@@ -93,26 +95,25 @@ class init_state(LeggedRobotCfg.init_state):
     class control(LeggedRobotCfg.control):
         # PD Drive parameters:
         stiffness = {
-            "x10": 200,
-            "x8": 200,
-            "x6": 200,
-            "x4": 200,
-            "foot": 200,
-            "forward": 200,
-            "knee": 200,
+            "shoulder": 200,
+            "elbow": 200,
+            "wrist": 200,
+            "hand": 200,
+            "torso": 200,
+            "hip": 200,
             "ankle": 200,
+            "knee": 200,
         }
         damping = {
-            "x10": 10,
-            "x8": 10,
-            "x6": 10,
-            "x4": 10,
-            "foot": 10,
-            "forward": 10,
-            "knee": 10,
+            "shoulder": 10,
+            "elbow": 10,
+            "wrist": 10,
+            "hand": 10,
+            "torso": 10,
+            "hip": 10,
             "ankle": 10,
+            "knee": 10,
         }
-
         # action scale: target angle = actionScale * action + defaultAngle
         action_scale = 0.25
         # decimation: Number of control action updates @ sim DT per policy DT

sim/humanoid_gym/envs/stompy_env.py

@@ -1,7 +1,7 @@
 # mypy: disable-error-code="valid-newtype"
 """Defines the environment for training the humanoid."""
 
-import torch
+import torch  # type: ignore[import]
 from humanoid.envs import LeggedRobot
 from humanoid.envs.base.legged_robot_config import LeggedRobotCfg
 from humanoid.utils.terrain import HumanoidTerrain
@@ -77,7 +77,6 @@ def _push_robots(self):
         self.rand_push_torque = torch_rand_float(
             -max_push_angular, max_push_angular, (self.num_envs, 3), device=self.device
         )
-
         self.root_states[:, 10:13] = self.rand_push_torque
 
         self.gym.set_actor_root_state_tensor(self.sim, gymtorch.unwrap_tensor(self.root_states))
@@ -123,16 +122,15 @@ def compute_ref_state(self):
         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"]] = sin_pos_l * scale_2
-        self.ref_dof_pos[:, self.legs_joints["left_ankle"]] = 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"]] = sin_pos_r * scale_2
-        self.ref_dof_pos[:, self.legs_joints["right_ankle"]] = 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
@@ -207,12 +205,10 @@ def compute_observations(self):
         contact_mask = self.contact_forces[:, self.feet_indices, 2] > 5.0
 
         self.command_input = torch.cat((sin_pos, cos_pos, self.commands[:, :3] * self.commands_scale), dim=1)
-        breakpoint()
         q = (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos
         dq = self.dof_vel * self.obs_scales.dof_vel
 
         diff = self.dof_pos - self.ref_dof_pos
-
         self.privileged_obs_buf = torch.cat(
             (
                 self.command_input,  # 2 + 3