fixed train

zeroth-robotics · Jan 13, 2025 · 45f1463 · 45f1463
1 parent aac9d54
commit 45f1463
Show file tree

Hide file tree

Showing 9 changed files with 334 additions and 270 deletions.
diff --git a/sim/envs/humanoids/stompymicro_config.py b/sim/envs/humanoids/stompymicro_config.py
@@ -1,7 +1,5 @@
 """Defines the environment configuration for the Getting up task"""
 
-import numpy as np
-
 from sim.env import robot_urdf_path
 from sim.envs.base.legged_robot_config import (  # type: ignore
     LeggedRobotCfg,
@@ -40,7 +38,8 @@ class asset(LeggedRobotCfg.asset):
         file = str(robot_urdf_path(name))
 
         foot_name = ["foot_left", "foot_right"]
-        knee_name = ["left_knee_pitch_motor", "right_knee_pitch_motor"]
+        # knee_name = ["left_knee_pitch_motor", "right_knee_pitch_motor"]
+        knee_name = ["ankle_pitch_left", "ankle_pitch_right"]
 
         termination_height = 0.05
         default_feet_height = 0.02
@@ -150,7 +149,7 @@ class ranges:
             lin_vel_x = [-0.05, 0.23]  # min max [m/s]
             lin_vel_y = [-0.04, 0.04]  # min max [m/s]
             ang_vel_yaw = [-0.1, 0.1]  # min max [rad/s]
-            heading = [-np.pi, np.pi]  # min max [rad]
+            heading = [-3.14, 3.14]
 
     class rewards:
         base_height_target = Robot.height

diff --git a/sim/resources/stompymicro/joints.py b/sim/resources/stompymicro/joints.py
@@ -97,14 +97,14 @@ class Robot(Node):
     def default_standing(cls) -> Dict[str, float]:
         return {
             # Legs
-            cls.legs.left.hip_pitch: 0.23,
-            cls.legs.left.knee_pitch: -0.741,
+            cls.legs.left.hip_pitch: 0.13,
+            cls.legs.left.knee_pitch: 0,#-0.741,
             cls.legs.left.hip_yaw: 0,
             cls.legs.left.hip_roll: 0,
-            cls.legs.left.ankle_pitch: -0.5,
-            cls.legs.right.hip_pitch: -0.23,
-            cls.legs.right.knee_pitch: 0.741,
-            cls.legs.right.ankle_pitch: 0.5,
+            cls.legs.left.ankle_pitch: 0,#-0.5,
+            cls.legs.right.hip_pitch: 0.13,
+            cls.legs.right.knee_pitch: 0,#0.741,
+            cls.legs.right.ankle_pitch: 0,#0.5,
             cls.legs.right.hip_yaw: 0,
             cls.legs.right.hip_roll: 0,
         }
@@ -121,10 +121,10 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
             #     "lower": -0.43633231,
             #     "upper": 1.5707963,
             # },
-            # Robot.left_arm.elbow_pitch: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
+            Robot.left_arm.elbow_pitch: {
+                "lower": 1.5707963,
+                "upper": 4.71,
+            },
             # # Right Arm
             # Robot.right_arm.shoulder_pitch: {
             #     "lower": -1.7453293,
@@ -134,10 +134,10 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
             #     "lower": -1.134464,
             #     "upper": 0.87266463,
             # },
-            # Robot.right_arm.elbow_pitch: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
+            Robot.right_arm.elbow_pitch: {
+                "lower": 1.5707963,
+                "upper": 4.71,
+            },
             # Left Leg
             Robot.legs.left.hip_pitch: {
                 "lower": -1.5707963,

diff --git a/sim/resources/stompymicro/meshes/foot_left.stl b/sim/resources/stompymicro/meshes/foot_left.stl
diff --git a/sim/resources/stompymicro/meshes/foot_right.stl b/sim/resources/stompymicro/meshes/foot_right.stl
diff --git a/sim/resources/stompymicro/meshes/hip_roll_left.stl b/sim/resources/stompymicro/meshes/hip_roll_left.stl
diff --git a/sim/resources/stompymicro/meshes/hip_roll_right.stl b/sim/resources/stompymicro/meshes/hip_roll_right.stl
diff --git a/sim/resources/stompymicro/robot.urdf b/sim/resources/stompymicro/robot.urdf
@@ -122,7 +122,7 @@
     </inertial>
   </link>
   <joint name="right_hip_pitch" type="revolute">
-    <origin xyz="-0.045750183202404315 -0.00032689585567163837 -0.10384882247403085" rpy="-1.570796353589792 -4.6410206566704915e-08 1.5707963000000003"/>
+    <origin xyz="-0.045750183202404315 -0.00032689585567163837 -0.09684882247403085" rpy="-1.570796353589792 -4.6410206566704915e-08 1.5707963000000003"/>
     <parent link="Torso"/>
     <child link="hip_yaw_right"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>
@@ -151,7 +151,7 @@
     </inertial>
   </link>
   <joint name="left_hip_pitch" type="revolute">
-    <origin xyz="0.045549819648300315 -0.0003268960191209584 -0.10384882247403085" rpy="1.5707963535897942 -4.6410206566704915e-08 1.5707963000000003"/>
+    <origin xyz="0.045549819648300315 -0.0003268960191209584 -0.09684882247403085" rpy="1.5707963535897942 -4.6410206566704915e-08 1.5707963000000003"/>
     <parent link="Torso"/>
     <child link="hip_yaw_left"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>
@@ -209,10 +209,10 @@
     </inertial>
   </link>
   <joint name="right_hip_yaw" type="revolute">
-    <origin xyz="-0.02550000162875169 0.012500001141551514 0.019359999918275368" rpy="9.282041333256968e-08 1.5707963000000005 0.0"/>
+    <origin xyz="-0.02550000162875169 0 0.019359999918275368" rpy="9.282041333256968e-08 1.5707963000000005 0.0"/>
     <parent link="hip_yaw_right"/>
     <child link="hip_roll_right"/>
-    <limit effort="80" velocity="5" lower="-0.087266463" upper="1.5707963"/>
+    <limit effort="80" velocity="5" lower="-0.3" upper="0.1"/>
     <axis xyz="0 0 1"/>
   </joint>
   <link name="hip_roll_right">
@@ -238,10 +238,10 @@
     </inertial>
   </link>
   <joint name="left_hip_yaw" type="revolute">
-    <origin xyz="-0.02550000297464769 -0.012500001 0.01936000008172467" rpy="-3.1415926071795863 -1.5707963 0.0"/>
+    <origin xyz="-0.02550000297464769 0 0.01936000008172467" rpy="-3.1415926071795863 -1.5707963 0.0"/>
     <parent link="hip_yaw_left"/>
     <child link="hip_roll_left"/>
-    <limit effort="80" velocity="5" lower="-1.5707963" upper="0.087266463"/>
+    <limit effort="80" velocity="5" lower="-0.1" upper="0.3"/>
     <axis xyz="0 0 1"/>
   </joint>
   <link name="hip_roll_left">
@@ -325,15 +325,15 @@
     </inertial>
   </link>
   <joint name="right_hip_roll" type="revolute">
-    <origin xyz="0.0 0.06776001387753285 0.015000000939161373" rpy="-1.5707963 0.0 0.0"/>
+    <origin xyz="0.0 0.07776001212246713 0.015000000939161373" rpy="-1.5707963 0.0 0.0"/>
     <parent link="hip_roll_right"/>
     <child link="knee_pitch_right"/>
     <limit effort="80" velocity="5" lower="-0.78539816" upper="0.78539816"/>
     <axis xyz="0 0 -1"/>
   </joint>
   <link name="knee_pitch_right">
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.01" rpy="0 0 0"/>
       <geometry>
         <mesh filename="meshes/knee_pitch_right.stl"/>
       </geometry>
@@ -354,15 +354,15 @@
     </inertial>
   </link>
   <joint name="left_hip_roll" type="revolute">
-    <origin xyz="0.0 0.06776001212246713 0.014999999060838654" rpy="-1.5707963071795865 0.0 0.0"/>
+    <origin xyz="0.0 0.07776001212246713 0.014999999060838654" rpy="-1.5707963071795865 0.0 0.0"/>
     <parent link="hip_roll_left"/>
     <child link="knee_pitch_left"/>
     <limit effort="80" velocity="5" lower="-0.78539816" upper="0.78539816"/>
     <axis xyz="0 0 -1"/>
   </joint>
   <link name="knee_pitch_left">
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.01" rpy="0 0 0"/>
       <geometry>
         <mesh filename="meshes/knee_pitch_left.stl"/>
       </geometry>
@@ -383,7 +383,7 @@
     </inertial>
   </link>
   <joint name="left_knee_pitch" type="revolute">
-    <origin xyz="0.018825 0 0.02221" rpy="1.5707963 0 -1.5707963"/>
+    <origin xyz="0.020825 0 0.03271" rpy="1.5707963 0 -1.5707963"/>
     <parent link="knee_pitch_left"/>
     <child link="ankle_pitch_left"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>
@@ -412,7 +412,7 @@
     </inertial>
   </link>
   <joint name="right_knee_pitch" type="revolute">
-    <origin xyz="-0.018825 0 0.02221" rpy="-1.5707963 0 -1.5707963"/>
+    <origin xyz="-0.020825 0 0.03271" rpy="-1.5707963 0 -1.5707963"/>
     <parent link="knee_pitch_right"/>
     <child link="ankle_pitch_right"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>
@@ -441,7 +441,7 @@
     </inertial>
   </link>
   <joint name="right_ankle_pitch" type="revolute">
-    <origin xyz="0.0 0 0" rpy="9.282041357749903e-08 0.0 0.0"/>
+    <origin xyz="0.0 -0.06 0" rpy="9.282041357749903e-08 0.0 0.0"/>
     <parent link="ankle_pitch_right"/>
     <child link="foot_right"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>
@@ -470,7 +470,7 @@
     </inertial>
   </link>
   <joint name="left_ankle_pitch" type="revolute">
-    <origin xyz="0.0 -0.07771999985844844 -0.000499991045895997" rpy="9.282041357749903e-08 0.0 0.0"/>
+    <origin xyz="0.0 0.06 0" rpy="9.282041357749903e-08 0.0 0.0"/>
     <parent link="ankle_pitch_left"/>
     <child link="foot_left"/>
     <limit effort="80" velocity="5" lower="-1.5707963" upper="1.5707963"/>