add kick test script

agent/agent.py

@@ -104,7 +104,7 @@ class Agent:
             self.agent.skills_manager.execute("Neutral")
         else:
             # self.execute_support_behavior()
-            self.agent.skills_manager.execute("Neutral")
+            self.kick_ball()
 
         self.agent.robot.commit_motor_targets_pd()
 
@@ -205,22 +205,22 @@ class Agent:
             b=target_point,
         )
 
+        kick_offset = MathOps.rotate_2d_vec(np.array([0.0, -0.1]), desired_orientation)
+
         close_to_ball = my_to_ball_dist <= 0.24
-        good_orientation = orientation_error <= 14.0
+        good_orientation = orientation_error <= 8.0
         good_lateral_offset = lateral_error_to_shot_line <= 0.16
-        in_shooting_range = ball_to_target_dist <= 12.0
 
         can_shoot = (
             close_to_ball
             and behind_ball
             and good_orientation
             and good_lateral_offset
-            and in_shooting_range
         )
 
         if not can_shoot:
-            prepare_offset = 0.06
+            prepare_offset = 0.1
-            prepare_pos = ball_pos - ball_to_target_dir * prepare_offset
+            prepare_pos = ball_pos - ball_to_target_dir * prepare_offset + kick_offset
 
             self.agent.skills_manager.execute(
                 "Walk",

behaviors/custom/keyframe/get_up/get_up.py

@@ -9,7 +9,7 @@ logger = logging.getLogger()
 
 class GetUp(Behavior):
     STABILITY_THRESHOLD_CYCLES: int = 3
-    NEUTRAL_EXECUTION_TIME: float = 1.0
+    NEUTRAL_EXECUTION_TIME: float = 1.5
     def __init__(self, agent):
         super().__init__(agent)
         self.get_up_front = KeyframeSkill(

behaviors/custom/keyframe/get_up/get_up_back.yaml

@@ -2,33 +2,6 @@ symmetry: true
 kp: 75
 kd: 1
 keyframes:
-  - delta: 1
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: 0
-      Shoulder_Roll: 0
-      Elbow_Pitch: 0
-      Elbow_Yaw: 0
-      Waist: 0.000
-      Hip_Pitch: 0
-      Hip_Roll: 0
-      Hip_Yaw: 0
-      Knee_Pitch: 0
-      Ankle_Pitch: 0
-      Ankle_Roll: 0
-    kp: 20
-    kd: 1
-    p_gains:
-      Hip_Pitch: 20
-      Hip_Roll: 20
-      Hip_Yaw: 20
-      Knee_Pitch: 20
-    d_gains:
-      Hip_Pitch: 1
-      Hip_Roll: 1
-      Hip_Yaw: 1
-      Knee_Pitch: 1
 
   - delta: 1
     motor_positions:
@@ -36,15 +9,15 @@ keyframes:
       Head_pitch: 0.000
       Shoulder_Pitch: -114.592
       Shoulder_Roll: -71.619
-      Elbow_Pitch: -100.944
+      Elbow_Pitch: -85.944
-      Elbow_Yaw: -171.000
+      Elbow_Yaw: -171.887
       Waist: 0.000
-      Hip_Pitch: -35.000
+      Hip_Pitch: -45.836
-      Hip_Roll: 24.000
+      Hip_Roll: 179.908
-      Hip_Yaw: 8.000
+      Hip_Yaw: 179.908
-      Knee_Pitch: -100.000
+      Knee_Pitch: -179.908
-      Ankle_Pitch: 70.000
+      Ankle_Pitch: 89.954
-      Ankle_Roll: 10.000
+      Ankle_Roll: 89.954
     kp: 250
     kd: 1
     p_gains:
@@ -58,91 +31,13 @@ keyframes:
       Hip_Yaw: 1
       Knee_Pitch: 1
 
-  - delta: 0.45
+  - delta: 0.8125
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.592
-      Shoulder_Roll: -80.000
-      Elbow_Pitch: -140.944
-      Elbow_Yaw: -171.000
-      Waist: 0.000
-      Hip_Pitch: -25.000
-      Hip_Roll: 20.000
-      Hip_Yaw: 8.000
-      Knee_Pitch: -120.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 8.000
-    kp: 220
-    kd: 1.5
-    p_gains:
-      Hip_Pitch: 20
-      Hip_Roll: 20
-      Hip_Yaw: 20
-      Knee_Pitch: 20
-    d_gains:
-      Hip_Pitch: 1
-      Hip_Roll: 1
-      Hip_Yaw: 1
-      Knee_Pitch: 1
-
-  - delta: 0.35
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.000
-      Shoulder_Roll: -86.000
-      Elbow_Pitch: -140.944
-      Elbow_Yaw: -171.000
-      Waist: 0.000
-      Hip_Pitch: -30.000
-      Hip_Roll: 160.000
-      Hip_Yaw: 160.000
-      Knee_Pitch: -150.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 60.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Hip_Pitch: 120
-      Hip_Roll: 120
-      Hip_Yaw: 120
-      Knee_Pitch: 180
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
-
-  - delta: 0.35
     motor_positions:
       Head_yaw: 0.000
       Head_pitch: 0.000
       Shoulder_Pitch: -114.592
       Shoulder_Roll: -89.954
-      Elbow_Pitch: -90.944
+      Elbow_Pitch: -85.944
-      Elbow_Yaw: 0.000
-      Waist: 0.000
-      Hip_Pitch: -30.000
-      Hip_Roll: 160.000
-      Hip_Yaw: 160.000
-      Knee_Pitch: -150.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 60.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Hip_Pitch: 120
-      Hip_Roll: 120
-      Hip_Yaw: 120
-      Knee_Pitch: 180
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
-  
-  - delta: 0.1
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.592
-      Shoulder_Roll: -89.954
-      Elbow_Pitch: -140.944
       Elbow_Yaw: 0.000
       Waist: 0.000
       Hip_Pitch: -45.836
@@ -151,38 +46,15 @@ keyframes:
       Knee_Pitch: -179.908
       Ankle_Pitch: 63.026
       Ankle_Roll: 45.836
-    kp: 50
-    kd: 2
     p_gains:
-      Hip_Pitch: 120
+      Hip_Pitch: 25
-      Hip_Roll: 120
+      Hip_Roll: 25
-      Hip_Yaw: 120
+      Hip_Yaw: 25
-      Knee_Pitch: 180
+      Knee_Pitch: 25
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
 
-  - delta: 0.15
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.000
-      Shoulder_Roll: -89.954
-      Elbow_Pitch: -45.000
-      Elbow_Yaw: 0.000
-      Waist: 0.000
-      Hip_Pitch: 10.000
-      Hip_Roll: 16.000
-      Hip_Yaw: 4.000
-      Knee_Pitch: -80.000
-      Ankle_Pitch: 30.000
-      Ankle_Roll: 6.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Ankle_Pitch: 80
-      Ankle_Roll: 80
 
-  - delta: 0.15
+
+  - delta: 0.6
     motor_positions:
       Head_yaw: 0.000
       Head_pitch: 0.000
@@ -197,8 +69,8 @@ keyframes:
       Knee_Pitch: -42.971
       Ankle_Pitch: 57.296
       Ankle_Roll: 0.000
-    kp: 50
+    kp: 40
-    kd: 2
+    kd: 4
     p_gains:
        Ankle_Pitch: 100
        Ankle_Roll: 100

behaviors/custom/keyframe/kick/kick_short.yaml

@@ -10,16 +10,16 @@ keyframes:
       Left_Shoulder_Roll: -76.000
       Left_Elbow_Pitch: -62.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
+      Right_Shoulder_Pitch: -72.000
       Right_Shoulder_Roll: 52.000
       Right_Elbow_Pitch: -70.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -18.000
+      Left_Hip_Pitch: -12.000
       Left_Hip_Roll: 8.000
       Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 42.000
+      Left_Knee_Pitch: 35.000
-      Left_Ankle_Pitch: 10.000
+      Left_Ankle_Pitch: -18.000
       Left_Ankle_Roll: 3.000
       Right_Hip_Pitch: 14.000
       Right_Hip_Roll: -9.000
@@ -36,16 +36,16 @@ keyframes:
       Left_Shoulder_Roll: -80.000
       Left_Elbow_Pitch: -70.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
+      Right_Shoulder_Pitch: -72.000
       Right_Shoulder_Roll: 50.000
       Right_Elbow_Pitch: -70.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -30.000
+      Left_Hip_Pitch: -25.000
       Left_Hip_Roll: 10.000
       Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 54.000
+      Left_Knee_Pitch: 45.000
-      Left_Ankle_Pitch: 10.000
+      Left_Ankle_Pitch: -20.000
       Left_Ankle_Roll: 4.500
       Right_Hip_Pitch: 14.000
       Right_Hip_Roll: -9.000
@@ -62,16 +62,16 @@ keyframes:
       Left_Shoulder_Roll: -82.000
       Left_Elbow_Pitch: -74.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
+      Right_Shoulder_Pitch: -72.000
       Right_Shoulder_Roll: 48.000
       Right_Elbow_Pitch: -68.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -46.000
+      Left_Hip_Pitch: -40.000
       Left_Hip_Roll: 12.000
       Left_Hip_Yaw: 2.000
-      Left_Knee_Pitch: 28.000
+      Left_Knee_Pitch: 18.000
-      Left_Ankle_Pitch: 10.000
+      Left_Ankle_Pitch: -24.000
       Left_Ankle_Roll: 5.000
       Right_Hip_Pitch: 14.000
       Right_Hip_Roll: -8.000
@@ -88,16 +88,16 @@ keyframes:
       Left_Shoulder_Roll: -84.000
       Left_Elbow_Pitch: -76.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
+      Right_Shoulder_Pitch: -72.000
       Right_Shoulder_Roll: 46.000
       Right_Elbow_Pitch: -66.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -58.000
+      Left_Hip_Pitch: -48.000
       Left_Hip_Roll: 13.000
       Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: -16.000
+      Left_Knee_Pitch: -8.000
-      Left_Ankle_Pitch: 8.000
+      Left_Ankle_Pitch: -18.000
       Left_Ankle_Roll: 5.000
       Right_Hip_Pitch: 12.000
       Right_Hip_Roll: -8.000
@@ -114,16 +114,16 @@ keyframes:
       Left_Shoulder_Roll: -86.000
       Left_Elbow_Pitch: -72.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 70.000
+      Right_Shoulder_Pitch: -70.000
       Right_Shoulder_Roll: 44.000
       Right_Elbow_Pitch: -64.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -62.000
+      Left_Hip_Pitch: -50.000
       Left_Hip_Roll: 13.000
       Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 10.000
+      Left_Knee_Pitch: 5.000
-      Left_Ankle_Pitch: 6.000
+      Left_Ankle_Pitch: -22.000
       Left_Ankle_Roll: 5.000
       Right_Hip_Pitch: 12.000
       Right_Hip_Roll: -8.000
@@ -141,16 +141,16 @@ keyframes:
       Left_Shoulder_Roll: -82.000
       Left_Elbow_Pitch: -69.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
+      Right_Shoulder_Pitch: -72.000
       Right_Shoulder_Roll: 48.000
       Right_Elbow_Pitch: -68.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -16.000
+      Left_Hip_Pitch: -8.000
       Left_Hip_Roll: 9.000
       Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 38.000
+      Left_Knee_Pitch: 28.000
-      Left_Ankle_Pitch: 9.000
+      Left_Ankle_Pitch: -22.000
       Left_Ankle_Roll: 3.000
       Right_Hip_Pitch: 8.000
       Right_Hip_Roll: -5.000
@@ -167,16 +167,16 @@ keyframes:
       Left_Shoulder_Roll: -82.000
       Left_Elbow_Pitch: -68.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 68.000
+      Right_Shoulder_Pitch: -68.000
       Right_Shoulder_Roll: 48.000
       Right_Elbow_Pitch: -62.000
       Right_Elbow_Yaw: 0.000
       Waist: 0.000
-      Left_Hip_Pitch: -10.000
+      Left_Hip_Pitch: -2.000
       Left_Hip_Roll: 9.000
       Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 34.000
+      Left_Knee_Pitch: 28.000
-      Left_Ankle_Pitch: 9.000
+      Left_Ankle_Pitch: -18.000
       Left_Ankle_Roll: 3.000
       Right_Hip_Pitch: 8.000
       Right_Hip_Roll: -5.000
@@ -193,7 +193,7 @@ keyframes:
       Left_Shoulder_Roll: -75.000
       Left_Elbow_Pitch: -60.000
       Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 65.000
+      Right_Shoulder_Pitch: -65.000
       Right_Shoulder_Roll: 45.000
       Right_Elbow_Pitch: -60.000
       Right_Elbow_Yaw: 0.000

kick_test.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+export OMP_NUM_THREADS=1
+
+host=${1:-localhost}
+port=${2:-60000}
+
+python3 run_player.py --host $host --port $port -n 1 -t SE &