add stompy v2

.gitignore

@@ -1,5 +1,8 @@
 # .gitignore
-
+# Robot Parts
+*.stl
+*.dae
+*.urdf
 # Local files
 .env
 # Python

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/deploy/run.py

@@ -14,11 +14,11 @@
 import numpy as np
 from humanoid.envs import *
 from humanoid.utils import task_registry
-from isaacgym import gymapi
 from isaacgym.torch_utils import *
 from policy import SimPolicy
 from tqdm import tqdm
 
+from isaacgym import gymapi
 from sim.deploy.config import RobotConfig
 from sim.env import stompy_mjcf_path
 from sim.stompy.joints import StompyFixed

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/getup_env.py

@@ -5,9 +5,9 @@
 from humanoid.envs import LeggedRobot
 from humanoid.envs.base.legged_robot_config import LeggedRobotCfg
 from humanoid.utils.terrain import HumanoidTerrain
-from isaacgym import gymtorch
 from isaacgym.torch_utils import *
 
+from isaacgym import gymtorch
 from sim.stompy.joints import Stompy
 
 default_feet_height = 0.0

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,13 +1,13 @@
 # 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
-from isaacgym import gymtorch
 from isaacgym.torch_utils import *
 
+from isaacgym import gymtorch
 from sim.stompy.joints import StompyFixed
 
 
@@ -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,13 +1,13 @@
 # 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
-from isaacgym import gymtorch
 from isaacgym.torch_utils import *
 
+from isaacgym import gymtorch
 from sim.stompy.joints import Stompy
 
 
@@ -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

sim/scripts/create_fixed_torso.py

@@ -5,7 +5,7 @@
 
 from sim.stompy.joints import StompyFixed
 
-STOMPY_URDF = "stompy/robot.urdf"
+STOMPY_URDF = "sim/stompy/robot.urdf"
 
 
 def update_urdf() -> None:
@@ -30,7 +30,7 @@ def update_urdf() -> None:
                 limit.set("upper", upper)
 
     # Save the modified URDF to a new file
-    tree.write("stompy/robot_fixed.urdf")
+    tree.write("sim/stompy/robot_fixed.urdf")
 
 
 if __name__ == "__main__":

sim/scripts/simulate_urdf.py

@@ -12,7 +12,6 @@
 from typing import Any, Dict, Literal, NewType
 
 from isaacgym import gymapi, gymtorch, gymutil
-
 from sim.env import stompy_urdf_path
 from sim.logging import configure_logging
 from sim.stompy.joints import Stompy
@@ -27,7 +26,7 @@
 Args = NewType("Args", Any)
 
 # Importing torch down here to avoid gymtorch issues.
-import torch  # noqa: E402
+import torch  # noqa: E402 #  type: ignore[import]
 
 # DRIVE_MODE = gymapi.DOF_MODE_EFFORT
 DRIVE_MODE = gymapi.DOF_MODE_POS
@@ -80,7 +79,7 @@ def load_gym() -> GymParams:
     sim_params.physx.num_threads = args.num_threads
     sim_params.physx.use_gpu = args.use_gpu
 
-    # sim_params.use_gpu_pipeline = False
+    sim_params.use_gpu_pipeline = False
     # if args.use_gpu_pipeline:
     #     warnings.warn("Forcing CPU pipeline.")
 
@@ -116,14 +115,14 @@ def load_gym() -> GymParams:
     asset_options = gymapi.AssetOptions()
     asset_options.default_dof_drive_mode = DRIVE_MODE
     asset_options.collapse_fixed_joints = True
-    asset_options.disable_gravity = True
-    asset_options.fix_base_link = True
+    asset_options.disable_gravity = False
+    asset_options.fix_base_link = False
     asset_path = stompy_urdf_path()
     robot_asset = gym.load_urdf(sim, str(asset_path.parent), str(asset_path.name), asset_options)
 
     # Adds the robot to the environment.
     initial_pose = gymapi.Transform()
-    initial_pose.p = gymapi.Vec3(0.0, 5.0, 0.0)
+    initial_pose.p = gymapi.Vec3(0.0, 2.0, 0.0)
     initial_pose.r = gymapi.Quat(0.0, 0.0, 0.0, 1.0)
     robot = gym.create_actor(env, robot_asset, initial_pose, "robot")
 
@@ -150,7 +149,7 @@ def load_gym() -> GymParams:
 
     # Resets the DOF positions to the starting positions.
     # dof_vel[:] = 0.0
-    starting_positions = Stompy.default_positions()
+    starting_positions = Stompy.default_standing()
     dof_ids: Dict[str, int] = gym.get_actor_dof_dict(env, robot)
     for joint_name, joint_position in starting_positions.items():
         dof_pos[0, dof_ids[joint_name]] = joint_position
@@ -188,7 +187,6 @@ def run_gym(gym: GymParams, mode: Literal["one_at_a_time", "all_at_once"] = "all
         gym.gym.step_graphics(gym.sim)
         gym.gym.draw_viewer(gym.viewer, gym.sim, True)
         gym.gym.sync_frame_time(gym.sim)
-
         # Print the joint forces.
         # print(gym.gym.get_actor_dof_forces(gym.env, gym.robot))
         # print(gym.gym.get_env_rigid_contact_forces(gym.env))