save more realistic efforts and velocities

kscalelabs · Jul 26, 2024 · 2aee560 · 2aee560
1 parent fd31715
commit 2aee560
Show file tree

Hide file tree

Showing 4 changed files with 58 additions and 58 deletions.
diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py
@@ -44,7 +44,7 @@ class asset(LeggedRobotCfg.asset):
         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
+        termination_height = 0.35
         default_feet_height = 0.0
         terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"]
 
@@ -108,11 +108,11 @@ class sim(LeggedRobotCfg.sim):
 
         class physx(LeggedRobotCfg.sim.physx):
             num_threads = 12
-            solver_type = 0  # 0: pgs, 1: tgs
+            solver_type = 0 # 0: pgs, 1: tgs
             num_position_iterations = 4
             num_velocity_iterations = 1
             contact_offset = 0.01  # [m]
-            rest_offset = 0.0  # [m]
+            rest_offset =-0.02  # [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
@@ -230,7 +230,7 @@ class runner:
         policy_class_name = "ActorCritic"
         algorithm_class_name = "PPO"
         num_steps_per_env = 60  # per iteration
-        max_iterations = 10001  # number of policy updates
+        max_iterations = 3001  # number of policy updates
 
         # logging
         save_interval = 100  # check for potential saves every this many iterations

diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py
@@ -53,4 +53,4 @@ def update_urdf() -> None:
 
 if __name__ == "__main__":
     update_urdf()
-    create_mjcf(STOMPY_URDF)
+    # create_mjcf(STOMPY_URDF + "/robot_fixed.urdf")
diff --git a/sim/stompy/joints.py b/sim/stompy/joints.py
@@ -153,65 +153,65 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
         return {
             # torso
             Stompy.torso.roll: {
-                "lower": -0.502,
+                "lower": -0.503,
                 "upper": -0.501,
             },
             # left arm
             Stompy.left_arm.shoulder_pitch: {
-                "lower": -0.251,
+                "lower": -0.252,
                 "upper": -0.250,
             },
             Stompy.left_arm.shoulder_yaw: {
-                "lower": 1.82,
+                "lower": 1.819,
                 "upper": 1.821,
             },
             Stompy.left_arm.shoulder_roll: {
-                "lower": -1.44,
+                "lower": -1.45,
                 "upper": -1.439,
             },
             Stompy.left_arm.elbow_pitch: {
-                "lower": 2.07,
+                "lower": 2.06,
                 "upper": 2.071,
             },
             Stompy.left_arm.hand.wrist_roll: {
-                "lower": -2.51,
+                "lower": -2.512,
                 "upper": -2.509,
             },
             Stompy.left_arm.hand.wrist_pitch: {
-                "lower": 3.33,
+                "lower": 3.32,
                 "upper": 3.331,
             },
             Stompy.left_arm.hand.wrist_yaw: {
-                "lower": 0.0628,
+                "lower": 0.062,
                 "upper": 0.0638,
             },
             # right arm
             Stompy.right_arm.shoulder_pitch: {
-                "lower": 2.7,
+                "lower": 2.69,
                 "upper": 2.701,
             },
             Stompy.right_arm.shoulder_yaw: {
-                "lower": -1.82,
+                "lower": -1.83,
                 "upper": -1.819,
             },
             Stompy.right_arm.shoulder_roll: {
-                "lower": -2.57,
+                "lower": -2.58,
                 "upper": -2.569,
             },
             Stompy.right_arm.elbow_pitch: {
-                "lower": -2.57,
+                "lower": -2.58,
                 "upper": -2.569,
             },
             Stompy.right_arm.hand.wrist_roll: {
-                "lower": 0,
+                "lower": -0.01,
                 "upper": 0.001,
             },
             Stompy.right_arm.hand.wrist_pitch: {
-                "lower": 0.251,
+                "lower": 0.250,
                 "upper": 0.252,
             },
             Stompy.right_arm.hand.wrist_yaw: {
-                "lower": 1.38,
+                "lower": 1.37,
                 "upper": 1.381,
             },
             # left leg
@@ -315,14 +315,14 @@ def effort(cls) -> Dict[str, float]:
             "knee pitch": 90,
             "ankle pitch": 24,
             "ankle roll": 24,
-            "shoulder pitch": 0,
-            "shoulder yaw": 0,
-            "shoulder roll": 0,
-            "elbow pitch": 0,
-            "wrist roll": 0,
-            "wrist pitch": 0,
-            "wrist yaw": 0,
-            "torso roll": 0,
+            "shoulder pitch":24,
+            "shoulder yaw": 24,
+            "shoulder roll": 24,
+            "elbow pitch": 24,
+            "wrist roll": 24,
+            "wrist pitch": 24,
+            "wrist yaw": 24,
+            "torso roll": 150,
         }
 
     # vel_limits
@@ -333,16 +333,16 @@ def velocity(cls) -> Dict[str, float]:
             "hip yaw": 40,
             "hip roll": 40,
             "knee pitch": 40,
-            "ankle pitch": 12,
-            "ankle roll": 12,
-            "shoulder pitch": 0,
-            "shoulder yaw": 0,
-            "shoulder roll": 0,
-            "elbow pitch": 0,
-            "wrist roll": 0,
-            "wrist pitch": 0,
-            "wrist yaw": 0,
-            "torso roll": 0,
+            "ankle pitch": 24,
+            "ankle roll": 24,
+            "shoulder pitch": 12,
+            "shoulder yaw": 12,
+            "shoulder roll": 12,
+            "elbow pitch": 12,
+            "wrist roll": 12,
+            "wrist pitch": 12,
+            "wrist yaw": 12,
+            "torso roll": 12,
         }
 
     @classmethod

diff --git a/sim/stompy/robot_fixed.urdf b/sim/stompy/robot_fixed.urdf
@@ -479,7 +479,7 @@
     <origin xyz="0.018560146 0.044808157 -0.0475" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x8_90_mock_1_outer_rmd_x8_90_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1" />
-    <limit effort="90" velocity="5" lower="-0.251" upper="-0.25" />
+    <limit effort="24" velocity="12" lower="-0.252" upper="-0.25" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1">
@@ -508,7 +508,7 @@
     <origin xyz="0.018560146 0.044808157 -0.0475" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_1_outer_rmd_x8_90_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1" />
-    <limit effort="90" velocity="5" lower="2.7" upper="2.701" />
+    <limit effort="24" velocity="12" lower="2.69" upper="2.701" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_torso_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1">
@@ -537,7 +537,7 @@
     <origin xyz="-0.018560146 0.044808157 -0.0475" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_torso_1_rmd_x8_90_mock_1_outer_rmd_x8_90_1" />
     <child link="link_upper_limb_assembly_7_dof_1_torso_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1" />
-    <limit effort="90" velocity="5" lower="-0.502" upper="-0.501" />
+    <limit effort="150" velocity="12" lower="-0.503" upper="-0.501" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_head_1_head_front_1">
@@ -863,7 +863,7 @@
     <origin xyz="0.018560146 0.044808157 -0.0475" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x8_90_mock_2_outer_rmd_x8_90_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x8_90_mock_2_inner_rmd_x8_90_1" />
-    <limit effort="90" velocity="5" lower="1.82" upper="1.821" />
+    <limit effort="24" velocity="12" lower="1.819" upper="1.821" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_2_inner_rmd_x8_90_1">
@@ -892,7 +892,7 @@
     <origin xyz="0.018560146 0.044808157 -0.0475" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_2_outer_rmd_x8_90_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x8_90_mock_2_inner_rmd_x8_90_1" />
-    <limit effort="90" velocity="5" lower="-1.82" upper="-1.819" />
+    <limit effort="24" velocity="12" lower="-1.83" upper="-1.819" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_part_1_1">
@@ -1386,7 +1386,7 @@
     <origin xyz="0 0 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="-1.44" upper="-1.439" />
+    <limit effort="24" velocity="12" lower="-1.45" upper="-1.439" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1">
@@ -1415,7 +1415,7 @@
     <origin xyz="0 0 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="-2.57" upper="-2.569" />
+    <limit effort="24" velocity="12" lower="-2.58" upper="-2.569" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_leg_assembly_left_1_rmd_x8_90_mock_1_inner_rmd_x8_90_1">
@@ -1826,7 +1826,7 @@
     <origin xyz="0.01225 0.021217622 -0.02" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x4_24_mock_2_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_upper_left_arm_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="2.07" upper="2.071" />
+    <limit effort="24" velocity="12" lower="2.06" upper="2.071" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1">
@@ -1855,7 +1855,7 @@
     <origin xyz="0.01225 0.021217622 -0.02" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_2_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_upper_left_arm_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="-2.57" upper="-2.569" />
+    <limit effort="24" velocity="12" lower="-2.58" upper="-2.569" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_leg_assembly_left_1_rmd_x8_90_mock_2_inner_rmd_x8_90_1">
@@ -2322,7 +2322,7 @@
     <origin xyz="0 0 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="-2.51" upper="-2.509" />
+    <limit effort="24" velocity="12" lower="-2.512" upper="-2.509" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1">
@@ -2351,7 +2351,7 @@
     <origin xyz="0 0 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="0" upper="0.001" />
+    <limit effort="24" velocity="12" lower="-0.01" upper="0.001" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_leg_assembly_left_1_leg_part_6_2">
@@ -2569,7 +2569,7 @@
     <origin xyz="-0.012380881 -0.0039640631 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_lower_limbs_1_leg_assembly_right_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_lower_limbs_1_leg_assembly_right_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="12" lower="0" upper="2" />
+    <limit effort="24" velocity="24" lower="0" upper="2" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_2_outer_rmd_x4_24_1">
@@ -2652,7 +2652,7 @@
     <origin xyz="-0.012380881 -0.0039640631 -0.025" rpy="3.1415927 0 0" />
     <parent link="link_lower_limbs_1_leg_assembly_left_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
     <child link="link_lower_limbs_1_leg_assembly_left_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
-    <limit effort="24" velocity="12" lower="-1.502" upper="0.498" />
+    <limit effort="24" velocity="24" lower="-1.502" upper="0.498" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_right_foot_1_ankle_half_2_right_2">
@@ -2708,7 +2708,7 @@
     <origin xyz="0.0245 0 -0.02" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_2_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="3.33" upper="3.331" />
+    <limit effort="24" velocity="12" lower="3.32" upper="3.331" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1">
@@ -2737,7 +2737,7 @@
     <origin xyz="0.0245 0 -0.02" rpy="3.1415927 0 0" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_2_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_2_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="0.251" upper="0.252" />
+    <limit effort="24" velocity="12" lower="0.25" upper="0.252" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_left_foot_1_ankle_half_2_left_1">
@@ -2955,7 +2955,7 @@
     <origin xyz="0 -0.0245 0.02" rpy="0 0 1.5707963" />
     <parent link="link_lower_limbs_1_right_foot_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
     <child link="link_lower_limbs_1_right_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="12" lower="0.76" upper="2.76" />
+    <limit effort="24" velocity="24" lower="0.76" upper="2.76" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_3_outer_rmd_x4_24_1">
@@ -3038,7 +3038,7 @@
     <origin xyz="0 0.0245 0.02" rpy="0 0 -1.5707963" />
     <parent link="link_lower_limbs_1_left_foot_1_rmd_x4_24_mock_1_outer_rmd_x4_24_1" />
     <child link="link_lower_limbs_1_left_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="12" lower="0.76" upper="2.76" />
+    <limit effort="24" velocity="24" lower="0.76" upper="2.76" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_right_foot_1_foot_1">
@@ -3094,7 +3094,7 @@
     <origin xyz="0 -0.0245 0.02" rpy="0 0 1.5707963" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_3_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_2_lower_arm_3_dof_1_rmd_x4_24_mock_3_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="0.0628" upper="0.0638" />
+    <limit effort="24" velocity="12" lower="0.062" upper="0.0638" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_3_inner_rmd_x4_24_1">
@@ -3123,7 +3123,7 @@
     <origin xyz="0 -0.0245 0.02" rpy="0 0 1.5707963" />
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_3_outer_rmd_x4_24_1" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_rmd_x4_24_mock_3_inner_rmd_x4_24_1" />
-    <limit effort="24" velocity="5" lower="1.38" upper="1.381" />
+    <limit effort="24" velocity="12" lower="1.37" upper="1.381" />
     <axis xyz="0 0 -1" />
     </joint>
   <link name="link_lower_limbs_1_left_foot_1_foot_1">
@@ -3815,4 +3815,4 @@
     <parent link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_hand_left_1_rack_4" />
     <child link="link_upper_limb_assembly_7_dof_1_full_arm_7_dof_1_lower_arm_3_dof_1_hand_left_1_inner_gripper_2" />
     </joint>
-  </robot>
+  </robot>