;-))

bit-bots · Dec 18, 2024 · bc0582b · bc0582b
1 parent a783722
commit bc0582b
Show file tree

Hide file tree

Showing 7 changed files with 144 additions and 162 deletions.
diff --git a/animations/grab_ball.json b/animations/grab_ball.json
diff --git a/...ior/behavior_dsd/actions/get_walkready.py → ...dy_behavior/behavior_dsd/actions/dynup.py b/...ior/behavior_dsd/actions/get_walkready.py → ...dy_behavior/behavior_dsd/actions/dynup.py
@@ -5,11 +5,23 @@
 from bitbots_msgs.action import Dynup
 
 
-class GetWalkready(AbstractActionElement):
+class PlayAnimationDynup(AbstractActionElement):
     blackboard: BodyBlackboard
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
+        self.direction = parameters.get("direction")
+        assert self.direction in [
+            Dynup.Goal.DIRECTION_FRONT,
+            Dynup.Goal.DIRECTION_FRONT_ONLY,
+            Dynup.Goal.DIRECTION_BACK,
+            Dynup.Goal.DIRECTION_BACK_ONLY,
+            Dynup.Goal.DIRECTION_RISE,
+            Dynup.Goal.DIRECTION_DESCEND,
+            Dynup.Goal.DIRECTION_WALKREADY,
+            Dynup.Goal.DIRECTION_RISE_NO_ARMS,
+            Dynup.Goal.DIRECTION_DESCEND_NO_ARMS,
+        ], f"Direction '{self.direction}' not supported"
         self.first_perform = True
         self.active = False
 
@@ -58,7 +70,7 @@ def start_animation(self) -> bool:
 
         # Dynup action server is running, we can start the walkready action
         goal = Dynup.Goal()
-        goal.direction = Dynup.Goal.DIRECTION_WALKREADY
+        goal.direction = self.direction
         self.active = True
         self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal)
         self.dynup_action_current_goal.add_done_callback(
@@ -78,3 +90,11 @@ def animation_finished(self) -> bool:
         :return: True if the animation is finished, False if not
         """
         return not self.active
+
+
+class GetWalkready(PlayAnimationDynup):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        parameters["direction"] = Dynup.Goal.DIRECTION_WALKREADY
+        super().__init__(blackboard, dsd, parameters)
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
@@ -121,7 +121,7 @@ $DoOnce
                             NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @AvoidBallActive, @GoToBall + target:gradient + distance:0.3 + blocking:false
                             DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.01 + latch:true
                                 NO --> @GoToBall + target:gradient + distance:0.3
-                                YES --> @LookForward, @ChangeAction + action:kicking, @Stand + duration:1.5 + r:false, @PlayAnimationGrabBall + r:false, @PlayAnimationThrow + r:false, @GetWalkready + r:false, @Stand
+                                YES --> @LookForward, @ChangeAction + action:kicking, @Stand + duration:1.5 + r:false, @PlayAnimationDynup + direction:descend_no_arms, @PlayAnimationGrabBall + r:false, @PlayAnimationDynup + direction:rise_no_arms, @PlayAnimationThrow + r:false, @GetWalkready + r:false, @Stand
                         SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToPassPreparePosition
                         THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
                     OTHER --> $RankToBallNoGoalie

diff --git a/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball.json b/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball.json
@@ -5,61 +5,41 @@
         {
             "duration": 3.0,
             "goals": {
-                "LAnklePitch": -51.44,
-                "LAnkleRoll": 12.66,
-                "LHipPitch": 71.54,
-                "LHipRoll": 13.54,
-                "LHipYaw": -1.23,
-                "LKnee": 134.74,
+                "LAnklePitch": -55.99,
+                "LAnkleRoll": 2.99,
+                "LElbow": -86.22,
+                "LHipPitch": 71.75,
+                "LHipRoll": 6.86,
+                "LHipYaw": -2.72,
+                "LKnee": 139.37,
+                "LShoulderPitch": -173.58,
+                "LShoulderRoll": -56.425784167551946,
                 "RAnklePitch": 51.44,
                 "RAnkleRoll": -12.66,
+                "RElbow": 82.66,
                 "RHipPitch": -71.54,
                 "RHipRoll": -13.54,
                 "RHipYaw": 1.23,
-                "RKnee": -134.74
+                "RKnee": -134.74,
+                "RShoulderPitch": 172.35,
+                "RShoulderRoll": 81.95
             },
             "name": "start frame",
             "pause": 0.0,
             "torque": {}
         },
         {
-            "duration": 1.2,
-            "goals": {
-                "LAnklePitch": -58.480000000000004,
-                "LAnkleRoll": 20.39,
-                "LElbow": -82.66,
-                "LHipPitch": 118.74,
-                "LHipRoll": 18.19,
-                "LHipYaw": -17.5,
-                "LKnee": 157.68,
-                "LShoulderPitch": -56.54,
-                "LShoulderRoll": -50.0,
-                "RAnklePitch": 58.480000000000004,
-                "RAnkleRoll": -20.39,
-                "RElbow": 82.66,
-                "RHipPitch": -118.74,
-                "RHipRoll": -18.19,
-                "RHipYaw": 17.5,
-                "RKnee": -157.68,
-                "RShoulderPitch": 56.54,
-                "RShoulderRoll": 50.0
-            },
-            "name": "Osu",
-            "pause": 0.0,
-            "torque": {}
-        },
-        {
-            "duration": 1.2,
+            "duration": 2.2,
             "goals": {
-                "LAnklePitch": -58.480000000000004,
-                "LAnkleRoll": 20.39,
-                "LElbow": -82.66,
-                "LHipPitch": 118.74,
-                "LHipRoll": 18.19,
-                "LHipYaw": -17.5,
-                "LKnee": 157.68,
-                "LShoulderPitch": -56.54,
-                "LShoulderRoll": 10.0,
+                "LAnklePitch": -57.48,
+                "LAnkleRoll": 11.96,
+                "LElbow": -86.22,
+                "LHipPitch": 121.61,
+                "LHipRoll": 14.250000000000002,
+                "LHipYaw": -3.08,
+                "LKnee": 160.21,
+                "LShoulderPitch": -58.89,
+                "LShoulderRoll": 19.35,
                 "RAnklePitch": 58.480000000000004,
                 "RAnkleRoll": -20.39,
                 "RElbow": 82.66,
@@ -68,24 +48,24 @@
                 "RHipYaw": 17.5,
                 "RKnee": -157.68,
                 "RShoulderPitch": 56.54,
-                "RShoulderRoll": -10.0
+                "RShoulderRoll": -19.66
             },
-            "name": "Scoop",
+            "name": "start frame_copy_1",
             "pause": 0.0,
             "torque": {}
         },
         {
             "duration": 1.2,
             "goals": {
-                "LAnklePitch": -59.71,
-                "LAnkleRoll": 15.560000000000002,
-                "LElbow": -82.66,
-                "LHipPitch": 70.05,
-                "LHipRoll": 17.67,
-                "LHipYaw": -3.17,
-                "LKnee": 139.31,
-                "LShoulderPitch": -164.0,
-                "LShoulderRoll": 10.0,
+                "LAnklePitch": -62.4,
+                "LAnkleRoll": 10.11,
+                "LElbow": -86.22,
+                "LHipPitch": 71.05,
+                "LHipRoll": 13.37,
+                "LHipYaw": 1.23,
+                "LKnee": 142.19,
+                "LShoulderPitch": -164.79,
+                "LShoulderRoll": 19.35,
                 "RAnklePitch": 59.71,
                 "RAnkleRoll": -15.560000000000002,
                 "RElbow": 82.66,
@@ -94,14 +74,14 @@
                 "RHipYaw": 3.17,
                 "RKnee": -139.31,
                 "RShoulderPitch": 164.0,
-                "RShoulderRoll": -10.0
+                "RShoulderRoll": -12.66
             },
-            "name": "Lift_ball",
+            "name": "start frame_copy_2",
             "pause": 0.0,
             "torque": {}
         }
     ],
-    "last_edited": "2024-11-27 17:07:25.777712",
+    "last_edited": "2024-12-11 18:35:30.256157",
     "name": "grab_ball",
     "version": ""
 }
diff --git a/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball_mk0.json b/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball_mk0.json
@@ -0,0 +1,69 @@
+{
+    "author": "Maxim, David, Okan",
+    "description": "Animation to grab the ball",
+    "keyframes": [
+
+        {
+            "duration": 1.2,
+            "goals": {
+                "LAnklePitch": -57.48,
+                "LAnkleRoll": 11.96,
+                "LElbow": -86.22,
+                "LHipPitch": 121.61,
+                "LHipRoll": 14.250000000000002,
+                "LHipYaw": -3.08,
+                "LKnee": 160.21,
+                "LShoulderPitch": -58.89,
+                "LShoulderRoll": -50.0,
+                "RAnklePitch": 58.480000000000004,
+                "RAnkleRoll": -20.39,
+                "RElbow": 82.66,
+                "RHipPitch": -118.74,
+                "RHipRoll": -18.19,
+                "RHipYaw": 17.5,
+                "RKnee": -157.68,
+                "RShoulderPitch": 56.54,
+                "RShoulderRoll": 50.0
+            },
+            "name": "Osu",
+            "pause": 0.0,
+            "torque": {}
+        },
+
+
+
+        {
+            "duration": 1.2,
+            "goals": {
+                "LElbow": -82.66,
+                "LShoulderPitch": -56.54,
+                "LShoulderRoll": 10.0,
+                "RElbow": 82.66,
+                "RShoulderPitch": 56.54,
+                "RShoulderRoll": -10.0
+            },
+            "name": "Scoop",
+            "pause": 0.0,
+            "torque": {}
+        },
+        {
+            "duration": 1.2,
+            "goals": {
+                "LElbow": -86.22,
+                "LShoulderPitch": -164.79,
+                "LShoulderRoll": 19.35,
+                "LHipPitch" : 0.0,
+                "RElbow": 82.66,
+                "RShoulderPitch": 164.0,
+                "RShoulderRoll": -12.66,
+                "RHipPitch": 0.0
+            },
+            "name": "Lift Ball",
+            "pause": 0.0,
+            "torque": {}
+        }
+    ],
+    "last_edited": "2024-12-11 16:50:13.653211",
+    "name": "grab_ball",
+    "version": ""
+}