update kps and friction (#131)

update general gpr parameters
kscalelabs · Dec 15, 2024 · 8a63381 · 8a63381
1 parent d9d6afa
commit 8a63381
Show file tree

Hide file tree

Showing 12 changed files with 82 additions and 84 deletions.
diff --git a/Makefile b/Makefile
@@ -43,13 +43,13 @@ play:
 # ------------------------ #
 
 install:
+	@pip install torch==2.3.1 torchvision==0.18.1 torchaudio==2.3.1 --index-url https://download.pytorch.org/whl/cu121
 	@pip install --verbose -e .
-	@bash sim/scripts/download_assets.sh
 .PHONY: install
 
 install-dev:
+	@pip install torch==2.3.1 torchvision==0.18.1 torchaudio==2.3.1 --index-url https://download.pytorch.org/whl/cu121
 	@pip install --verbose -e '.[dev]'
-	@bash sim/scripts/download_assets.sh
 .PHONY: install
 
 install-third-party:

diff --git a/examples/gpr_standing.kinfer b/examples/gpr_standing.kinfer
diff --git a/examples/gpr_walking.kinfer b/examples/gpr_walking.kinfer
diff --git a/policy.pt b/policy.pt
diff --git a/sim/envs/base/legged_robot.py b/sim/envs/base/legged_robot.py
@@ -579,7 +579,7 @@ def _init_buffers(self):
             if not found:
                 self.p_gains[:, i] = 0.0
                 self.d_gains[:, i] = 0.0
-                print(f"PD gain of joint {name} were not defined, setting them to zero")
+                raise ValueError(f"PD gain of joint {name} were not defined, setting them to zero")
 
         self.rand_push_force = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)
         self.rand_push_torque = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)

diff --git a/sim/envs/humanoids/gpr_config.py b/sim/envs/humanoids/gpr_config.py
@@ -51,8 +51,8 @@ class asset(LeggedRobotCfg.asset):
         fix_base_link = False
 
     class terrain(LeggedRobotCfg.terrain):
-        mesh_type = "plane"
-        # mesh_type = "trimesh"
+        # mesh_type = "plane"
+        mesh_type = "trimesh"
         curriculum = False
         # rough terrain only:
         measure_heights = False
@@ -122,7 +122,7 @@ class domain_rand(LeggedRobotCfg.domain_rand):
         friction_range = [0.1, 2.0]
 
         randomize_base_mass = True
-        added_mass_range = [-3.0, 3.0]
+        added_mass_range = [-2.0, 2.0]
         push_robots = True
         push_interval_s = 4
         max_push_vel_xy = 0.2
@@ -152,7 +152,7 @@ class rewards:
         target_joint_pos_scale = 0.17  # rad
         target_feet_height = 0.05  # m
 
-        cycle_time = 0.25  # sec
+        cycle_time = 0.4  # sec
         # if true negative total rewards are clipped at zero (avoids early termination problems)
         only_positive_rewards = True
         # tracking reward = exp(error*sigma)
@@ -162,10 +162,10 @@ class rewards:
         class scales:
             # reference motion tracking
             joint_pos = 1.6
-            feet_clearance = 1.0
-            feet_contact_number = 1.2
+            feet_clearance = 1.2
+            feet_contact_number = 1.4
             # gait
-            feet_air_time = 1.0
+            feet_air_time = 1.2
             foot_slip = -0.05
             feet_distance = 0.2
             knee_distance = 0.2
@@ -186,8 +186,8 @@ class scales:
             # energy
             action_smoothness = -0.002
             torques = -1e-5
-            dof_vel = -5e-4
-            dof_acc = -1e-7
+            dof_vel = -5e-4  # -1e-3
+            dof_acc = -1e-7  # -2.5e-7
             collision = -1.0
 
     class normalization:
@@ -211,20 +211,25 @@ class viewer:
 class GprStandingCfg(GprCfg):
     """Configuration class for the GPR humanoid robot."""
 
+    class init_state(LeggedRobotCfg.init_state):
+        pos = [0.0, 0.0, Robot.standing_height]
+        rot = Robot.rotation
+        default_joint_angles = {k: 0.0 for k in Robot.all_joints()}
+
     class rewards:
         # quite important to keep it right
-        base_height_target = Robot.height
+        base_height_target = Robot.standing_height
         min_dist = 0.2
         max_dist = 0.5
         # put some settings here for LLM parameter tuning
         target_joint_pos_scale = 0.17  # rad
         target_feet_height = 0.05  # m
         cycle_time = 0.5  # sec
         # if true negative total rewards are clipped at zero (avoids early termination problems)
-        only_positive_rewards = True
+        only_positive_rewards = False
         # tracking reward = exp(error*sigma)
         tracking_sigma = 5
-        max_contact_force = 500  # forces above this value are penalized
+        max_contact_force = 200  # forces above this value are penalized
 
         class scales:
             # base pos
@@ -235,8 +240,8 @@ class scales:
             # energy
             action_smoothness = -0.002
             torques = -1e-5
-            dof_vel = -5e-4
-            dof_acc = -1e-7
+            dof_vel = -1e-3
+            dof_acc = -2.5e-7
             collision = -1.0
 
 

diff --git a/sim/requirements.txt b/sim/requirements.txt
@@ -11,4 +11,5 @@ wandb
 tensorboard==2.14.0
 onnxscript
 mujoco==2.3.6
-kinfer
+kinfer==0.0.5
+opencv-python
diff --git a/sim/resources/gpr/joints.py b/sim/resources/gpr/joints.py
@@ -72,6 +72,7 @@ class Robot(Node):
     legs = Legs()
 
     height = 1.05
+    standing_height = 1.05 + 0.025
     rotation = [0, 0, 0, 1]
 
     @classmethod
@@ -106,7 +107,18 @@ def isaac_to_mujoco_signs(cls) -> Dict[str, int]:
 
     @classmethod
     def default_positions(cls) -> Dict[str, float]:
-        return {}
+        return {
+            Robot.legs.left.hip_pitch: 0.0,
+            Robot.legs.left.hip_yaw: 0.0,
+            Robot.legs.left.hip_roll: 0.0,
+            Robot.legs.left.knee_pitch: 0.0,
+            Robot.legs.left.ankle_pitch: 0.0,
+            Robot.legs.right.hip_pitch: 0.0,
+            Robot.legs.right.hip_yaw: 0.0,
+            Robot.legs.right.hip_roll: 0.0,
+            Robot.legs.right.knee_pitch: 0.0,
+            Robot.legs.right.ankle_pitch: 0.0,
+        }
 
     # CONTRACT - this should be ordered according to how the policy is trained.
     # E.g. the first entry should be the angle of the first joint in the policy.
@@ -136,36 +148,36 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
     @classmethod
     def stiffness(cls) -> Dict[str, float]:
         return {
-            "hip_y": 120,
-            "hip_x": 60,
-            "hip_z": 60,
-            "knee": 120,
-            "ankle_y": 17,
+            "hip_y": 300,
+            "hip_x": 120,
+            "hip_z": 120,
+            "knee": 300,
+            "ankle_y": 40,
         }
 
     # d_gains
     @classmethod
     def damping(cls) -> Dict[str, float]:
         return {
-            "hip_y": 10,
+            "hip_y": 5,
             "hip_x": 5,
             "hip_z": 5,
-            "knee": 10,
+            "knee": 5,
             "ankle_y": 5,
         }
 
-    # # pos_limits
+    # effort_limits
     @classmethod
     def effort(cls) -> Dict[str, float]:
         return {
-            "hip_y": 40,
-            "hip_x": 20,
-            "hip_z": 20,
-            "knee": 40,
+            "hip_y": 60,
+            "hip_x": 40,
+            "hip_z": 40,
+            "knee": 60,
             "ankle_y": 17,
         }
 
-    # # vel_limits
+    # vel_limits
     @classmethod
     def velocity(cls) -> Dict[str, float]:
         return {"hip": 10, "knee": 10, "ankle": 10}
@@ -175,7 +187,7 @@ def friction(cls) -> Dict[str, float]:
         return {
             "hip": 0,
             "knee": 0,
-            "ankle": 0.01,
+            "ankle": 0.1,
         }
 
 

diff --git a/sim/resources/gpr/robot_fixed.urdf b/sim/resources/gpr/robot_fixed.urdf
@@ -95,9 +95,9 @@
     <origin xyz="-0.008010608503738428 -0.43201043179234816 0.0877000068648673" rpy="-3.1415925071795874 4.641020678874952e-08 -3.1415926535897913" />
     <parent link="body1-part" />
     <child link="leg0_shell" />
-    <limit effort="40" velocity="10" lower="-1.5707963" upper="1.5707963" />
+    <limit effort="60" velocity="10" lower="-1.5707963" upper="1.5707963" />
     <axis xyz="0 0 -1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg0_shell">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -124,9 +124,9 @@
     <origin xyz="-0.008010600340183055 -0.4320104467591277 -0.08819999313513187" rpy="9.99999991702083e-08 4.641020678874952e-08 -3.1415926535897913" />
     <parent link="body1-part" />
     <child link="leg0_shell_2" />
-    <limit effort="40" velocity="10" lower="-1.5707963" upper="1.5707963" />
+    <limit effort="60" velocity="10" lower="-1.5707963" upper="1.5707963" />
     <axis xyz="0 0 -1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg0_shell_2">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -211,9 +211,9 @@
     <origin xyz="0.02649999999999997 -1.5165911463888016e-09 -0.06950000151659115" rpy="-1.5707963 0.0 1.5707963" />
     <parent link="leg0_shell" />
     <child link="leg1_shell" />
-    <limit effort="20" velocity="10" lower="-0.34906585" upper="3.1852259" />
+    <limit effort="40" velocity="10" lower="-0.34906585" upper="3.1852259" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg1_shell">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -240,9 +240,9 @@
     <origin xyz="0.02649999999999997 -1.5165911463888016e-09 -0.06950000151659115" rpy="1.5707963 -4.641020634466031e-08 -1.5707963535897922" />
     <parent link="leg0_shell_2" />
     <child link="leg1_shell3" />
-    <limit effort="20" velocity="10" lower="-0.34906585" upper="3.1852259" />
+    <limit effort="40" velocity="10" lower="-0.34906585" upper="3.1852259" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg1_shell3">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -327,9 +327,9 @@
     <origin xyz="-0.15649999999999997 0.0 0.027499998483408852" rpy="-0.0 1.5707963 0.0" />
     <parent link="leg1_shell" />
     <child link="leg2_shell" />
-    <limit effort="20" velocity="10" lower="-1.5707963" upper="1.5707963" />
+    <limit effort="40" velocity="10" lower="-1.5707963" upper="1.5707963" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg2_shell">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -356,9 +356,9 @@
     <origin xyz="-0.15649999999999997 0.0 0.027499998483408852" rpy="-3.1415926535897922 -1.5707962732050302 0.0" />
     <parent link="leg1_shell3" />
     <child link="leg2_shell_2" />
-    <limit effort="20" velocity="10" lower="-1.5707963" upper="1.5707963" />
+    <limit effort="40" velocity="10" lower="-1.5707963" upper="1.5707963" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg2_shell_2">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -443,9 +443,9 @@
     <origin xyz="0.0 0.0342 -0.14250000009378214" rpy="1.5707963000000003 0.0 0.0" />
     <parent link="leg2_shell" />
     <child link="leg3_shell2" />
-    <limit effort="40" velocity="10" lower="-1.57" upper="0" />
+    <limit effort="60" velocity="10" lower="-1.57" upper="0" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg3_shell2">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -472,9 +472,9 @@
     <origin xyz="0.0 0.0342 -0.14250000009378214" rpy="1.5707963000000003 0.0 0.0" />
     <parent link="leg2_shell_2" />
     <child link="leg3_shell22" />
-    <limit effort="40" velocity="10" lower="0" upper="1.57" />
+    <limit effort="60" velocity="10" lower="0" upper="1.57" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0" /></joint>
   <link name="leg3_shell22">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -561,7 +561,7 @@
     <child link="foot3" />
     <limit effort="17" velocity="10" lower="-0.6981317" upper="0.6981317" />
     <axis xyz="0 0 1" />
-  </joint>
+  <dynamics damping="0.0" friction="0.1" /></joint>
   <link name="foot3">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
@@ -590,7 +590,7 @@
     <child link="foot1" />
     <limit effort="17" velocity="10" lower="-0.6981317" upper="0.6981317" />
     <axis xyz="0 0 -1" />
-  </joint>
+  <dynamics damping="0.0" friction="0.1" /></joint>
   <link name="foot1">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />

diff --git a/sim/resources/gpr/robot_fixed.xml b/sim/resources/gpr/robot_fixed.xml
@@ -50,7 +50,7 @@
     <camera name="track" mode="trackcom" pos="0 -3.0 0.9010549999999999" xyaxes="1 0 0 0 0 1" />
     <body name="root" pos="0 0 0.411055" quat="1 0 0 0">
       <freejoint name="root"/>
-      <site name="imu" size="0.01" pos="0 0 0" />
+      <site name="imu" size="0.01" pos="0 0 -0.13"/>
       <body name="base">
         <inertial pos="0.00648939 0.00390843 -0.180571" quat="0.999495 -0.0317223 -0.00110485 0.00149824" mass="18.9034" diaginertia="1.33012 0.801658 0.559678" />
         <geom quat="0.000563312 -0.000562864 0.707388 0.706825" type="mesh" rgba="1 1 1 1" mesh="body1-part" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
@@ -189,5 +189,6 @@
 
   <keyframe>
     <key name="default" qpos="0 0 1.05 1. 0.0 0.0 0.0 -0.23 0.0 0.0 -0.441 -0.195 0.23 0.0 0.0 0.441 0.195"/>
+    <key name="standing" qpos="0 0 1.075 1. 0.0 0.0 0.0 0 0.0 0.0 0 0 0 0.0 0.0 0 0"/>
   </keyframe>
 </mujoco>
diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py
@@ -54,6 +54,17 @@ def update_urdf(model_path: str, embodiment: str) -> None:
                 for key, value in friction.items():
                     if key in joint_name:  # type: ignore[operator]
                         dynamics.set("friction", str(value))
+            else:
+                # Create and add new dynamics element
+                dynamics = ET.SubElement(joint, "dynamics")
+                dynamics.set("damping", "0.0")
+                # Set friction if exists for this joint
+                for key, value in friction.items():
+                    if key in joint_name:  # type: ignore[operator]
+                        dynamics.set("friction", str(value))
+                        break
+                else:
+                    dynamics.set("friction", "0.0")
 
     # Save the modified URDF to a new file
     tree.write(Path(model_path) / "robot_fixed.urdf", xml_declaration=False)

diff --git a/sim/scripts/download_assets.sh b/sim/scripts/download_assets.sh