Full robot (#35)

* new stompy, joints, config

* some high level init and env stuff

* bump gitignore

* added mjcf version of full stompy, some script improvements

* better mjcf version

* cleanup random urdf in meshes dir

.gitignore

@@ -27,4 +27,3 @@ out*/
 # Training artifacts
 wandb/
 runs/
-stompy/

sim/env.py

@@ -16,7 +16,7 @@ def stompy_urdf_path(legs_only: bool = False) -> Path:
     if legs_only:
         stompy_path = model_dir() / "robot_fixed.urdf"
     else:
-        stompy_path = model_dir() / "robot.urdf"
+        stompy_path = model_dir() / "robot_fixed.urdf"
 
     if not stompy_path.exists():
         raise FileNotFoundError(f"URDF file not found: {stompy_path}")
@@ -28,7 +28,7 @@ def stompy_mjcf_path(legs_only: bool = False) -> Path:
     if legs_only:
         stompy_path = model_dir() / "robot_fixed.xml"
     else:
-        stompy_path = model_dir() / "robot.xml"
+        stompy_path = model_dir() / "robot_fixed.xml"
 
     if not stompy_path.exists():
         raise FileNotFoundError(f"MJCF file not found: {stompy_path}")

sim/humanoid_gym/envs/__init__.py

@@ -11,14 +11,10 @@
 import isaacgym # isort:skip
 import torch #isort:skip
 # 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
-from .only_legs_env import OnlyLegsFreeEnv
+# from .getup_config import GetupCfg, GetupCfgPPO
+# from .getup_env import GetupFreeEnv
+# from .only_legs_config import OnlyLegsCfg, OnlyLegsCfgPPO
+# from .only_legs_env import OnlyLegsFreeEnv
 from .stompy_config import StompyCfg, StompyCfgPPO
 from .stompy_env import StompyFreeEnv
 
@@ -32,9 +28,8 @@ def register_tasks() -> None:
     from humanoid.utils.task_registry import task_registry
 
     task_registry.register("stompy_ppo", StompyFreeEnv, StompyCfg(), StompyCfgPPO())
-    task_registry.register("getup_ppo", GetupFreeEnv, GetupCfg(), GetupCfgPPO())
-    task_registry.register("legs_ppo", LegsFreeEnv, LegsCfg(), LegsCfgPPO())
-    task_registry.register("only_legs_ppo", OnlyLegsFreeEnv, OnlyLegsCfg(), OnlyLegsCfgPPO())
+    # task_registry.register("getup_ppo", GetupFreeEnv, GetupCfg(), GetupCfgPPO())
+    # task_registry.register("only_legs_ppo", OnlyLegsFreeEnv, OnlyLegsCfg(), OnlyLegsCfgPPO())
 
 
 register_tasks()

sim/humanoid_gym/envs/stompy_config.py

@@ -34,7 +34,7 @@ class safety:
         # safety factors
         pos_limit = 1.0
         vel_limit = 1.0
-        torque_limit = 1.0
+        torque_limit = 0.85
 
     class asset(LeggedRobotCfg.asset):
         file = str(stompy_urdf_path())
@@ -49,7 +49,7 @@ class asset(LeggedRobotCfg.asset):
         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
+        self_collisions = 0  # 1 to disable, 0 to enable...bitwise filter
         flip_visual_attachments = False
         replace_cylinder_with_capsule = False
         fix_base_link = False
@@ -84,8 +84,8 @@ class noise_scales:
             height_measurements = 0.1
 
     class init_state(LeggedRobotCfg.init_state):
-        pos = [0.0, 0.0, 1.15]
-
+        pos = [0.0, 0.0, 1.1]
+        rot = [0.0, 0.0, 0.7071068, 0.7071068]
         default_joint_angles = {k: 0.0 for k in Stompy.all_joints()}
 
         default_positions = Stompy.default_standing()
@@ -94,26 +94,8 @@ class init_state(LeggedRobotCfg.init_state):
 
     class control(LeggedRobotCfg.control):
         # PD Drive parameters:
-        stiffness = {
-            "shoulder": 200,
-            "elbow": 200,
-            "wrist": 200,
-            "hand": 200,
-            "torso": 200,
-            "hip": 200,
-            "ankle": 200,
-            "knee": 200,
-        }
-        damping = {
-            "shoulder": 10,
-            "elbow": 10,
-            "wrist": 10,
-            "hand": 10,
-            "torso": 10,
-            "hip": 10,
-            "ankle": 10,
-            "knee": 10,
-        }
+        stiffness = Stompy.stiffness()
+        damping = Stompy.damping()
         # action scale: target angle = actionScale * action + defaultAngle
         action_scale = 0.25
         # decimation: Number of control action updates @ sim DT per policy DT
@@ -126,11 +108,11 @@ class sim(LeggedRobotCfg.sim):
 
         class physx(LeggedRobotCfg.sim.physx):
             num_threads = 12
-            solver_type = 1  # 0: pgs, 1: tgs
+            solver_type = 0  # 0: pgs, 1: tgs
             num_position_iterations = 4
-            num_velocity_iterations = 0
+            num_velocity_iterations = 1
             contact_offset = 0.01  # [m]
-            rest_offset = -0.02  # [m]
+            rest_offset = 0.0  # [m]
             bounce_threshold_velocity = 0.1  # [m/s]
             max_depenetration_velocity = 1.0
             max_gpu_contact_pairs = 2**23  # 2**24 -> needed for 8000 envs and more
@@ -144,7 +126,7 @@ class domain_rand:
 
         randomize_base_mass = True
         added_mass_range = [-1.0, 1.0]
-        push_robots = True
+        push_robots = False
         push_interval_s = 4
         max_push_vel_xy = 0.2
         max_push_ang_vel = 0.4
@@ -178,23 +160,23 @@ class rewards:
         max_contact_force = 400  # forces above this value are penalized
 
         class scales:
-            # # reference motion tracking
-            # joint_pos = 1.6
-            # feet_clearance = 1.0
-            # feet_contact_number = 1.2
-            # # gait
-            # feet_air_time = 1.0
-            # foot_slip = -0.05
-            # feet_distance = 0.2
-            # knee_distance = 0.2
-            # # contact
-            # feet_contact_forces = -0.01
-            # # vel tracking
-            # tracking_lin_vel = 1.2
-            # tracking_ang_vel = 1.1
-            # vel_mismatch_exp = 0.5  # lin_z; ang x,y
-            # low_speed = 0.2
-            # track_vel_hard = 0.5
+            # reference motion tracking
+            joint_pos = 1.6
+            feet_clearance = 1.0
+            feet_contact_number = 1.2
+            # gait
+            feet_air_time = 1.0
+            foot_slip = -0.05
+            feet_distance = 0.2
+            knee_distance = 0.2
+            # contact
+            feet_contact_forces = -0.01
+            # vel tracking
+            tracking_lin_vel = 1.2
+            tracking_ang_vel = 1.1
+            vel_mismatch_exp = 0.5  # lin_z; ang x,y
+            low_speed = 0.2
+            track_vel_hard = 0.5
 
             # above this was removed
             # base pos
@@ -252,7 +234,7 @@ class runner:
 
         # logging
         save_interval = 100  # check for potential saves every this many iterations
-        experiment_name = "Legs"
+        experiment_name = "Full"
         run_name = ""
         # load and resume
         resume = False

sim/scripts/create_mjcf.py

@@ -49,7 +49,7 @@ def _pretty_print_xml(xml_string: str) -> str:
 class Sim2SimRobot(mjcf.Robot):
     """A class to adapt the world in a Mujoco XML file."""
 
-    def adapt_world(self, add_floor: bool = True, remove_frc_range: bool = False) -> None:
+    def adapt_world(self, add_floor: bool = True, remove_frc_range: bool = True) -> None:
         root: ET.Element = self.tree.getroot()
 
         if add_floor:
@@ -239,12 +239,12 @@ def adapt_world(self, add_floor: bool = True, remove_frc_range: bool = False) ->
                 index = list(body).index(geom)
                 body.insert(index + 1, new_geom)
 
-        if remove_frc_range:
-            for body in root.findall(".//body"):
-                joints = list(body.findall("joint"))
-                for join in joints:
-                    if "actuatorfrcrange" in join.attrib:
-                        join.attrib.pop("actuatorfrcrange")
+        for body in root.findall(".//body"):
+            joints = list(body.findall("joint"))
+            for join in joints:
+                print(join)
+                if "actuatorfrcrange" in join.attrib:
+                    join.attrib.pop("actuatorfrcrange")
 
         # Adding keyframe
         default_standing = stompy.default_standing()