From 2568fc1a9ad6273791d3717853efd7ffaaabeb4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BE=90=E5=AD=A6=E9=A2=A2?= Date: Sat, 18 Apr 2026 18:42:06 +0800 Subject: [PATCH] add keyframe kick behavior, but still need to be improved --- .gitignore | 1 - agent/agent.py | 174 +++++++++++++- behaviors/behavior_manager.py | 3 +- behaviors/custom/keyframe/kick/kick.py | 91 ++++++++ behaviors/custom/keyframe/kick/kick_10m.yaml | 187 +++++++++++++++ behaviors/custom/keyframe/kick/kick_long.yaml | 213 ++++++++++++++++++ behaviors/custom/keyframe/kick/kick_mid.yaml | 213 ++++++++++++++++++ .../custom/keyframe/kick/kick_short.yaml | 213 ++++++++++++++++++ 8 files changed, 1081 insertions(+), 14 deletions(-) create mode 100644 behaviors/custom/keyframe/kick/kick.py create mode 100644 behaviors/custom/keyframe/kick/kick_10m.yaml create mode 100644 behaviors/custom/keyframe/kick/kick_long.yaml create mode 100644 behaviors/custom/keyframe/kick/kick_mid.yaml create mode 100644 behaviors/custom/keyframe/kick/kick_short.yaml diff --git a/.gitignore b/.gitignore index 21013b5..74a69de 100644 --- a/.gitignore +++ b/.gitignore @@ -15,7 +15,6 @@ dist/ *.npz *.xml *.json -*.yaml *.iml *.pyc .TXT diff --git a/agent/agent.py b/agent/agent.py index 60ab684..6efe0d8 100644 --- a/agent/agent.py +++ b/agent/agent.py @@ -49,6 +49,9 @@ class Agent: } } + TEAMMATE_INFO_MAX_AGE: float = 0.8 + SUPPORT_DISTANCE_FROM_BALL: float = 1.2 + def __init__(self, agent): """ Creates a new DecisionMaker linked to the given agent. @@ -60,6 +63,10 @@ class Agent: self.agent: Base_Agent = agent self.is_getting_up: bool = False + self.is_kicking: bool = False + self.current_shot_target_point: np.ndarray | None = None + self.target_point: np.ndarray = np.array([0.0, 0.0]) + self.my_pos: np.ndarray = self.agent.world.global_position[:2] def update_current_behavior(self) -> None: """ @@ -85,11 +92,19 @@ class Agent: self.is_getting_up = not self.agent.skills_manager.execute(skill_name="GetUp") elif self.agent.world.playmode is PlayModeEnum.PLAY_ON: - self.carry_ball() + active_player_number = self.get_active_player_number() + if active_player_number == self.agent.world.number: + # self.carry_ball() + self.target_point = self.my_pos + np.array([4.0, 0.0]) + self.kick_ball() + else: + # self.execute_support_behavior() + self.agent.skills_manager.execute("Neutral") elif self.agent.world.playmode in (PlayModeEnum.BEFORE_KICK_OFF, PlayModeEnum.THEIR_GOAL, PlayModeEnum.OUR_GOAL): self.agent.skills_manager.execute("Neutral") else: - self.carry_ball() + # self.execute_support_behavior() + self.agent.skills_manager.execute("Neutral") self.agent.robot.commit_motor_targets_pd() @@ -97,18 +112,21 @@ class Agent: """ Basic example of a behavior: moves the robot toward the goal while handling the ball. """ - their_goal_pos = self.agent.world.field.get_their_goal_position()[:2] + target_point = self.target_point ball_pos = self.agent.world.ball_pos[:2] my_pos = self.agent.world.global_position[:2] - ball_to_goal = their_goal_pos - ball_pos - bg_norm = np.linalg.norm(ball_to_goal) - if bg_norm == 0: + if self.kick_ball(target_point=target_point): return - ball_to_goal_dir = ball_to_goal / bg_norm + + ball_to_target = target_point - ball_pos + bt_norm = np.linalg.norm(ball_to_target) + if bt_norm == 0: + return + ball_to_target_dir = ball_to_target / bt_norm dist_from_ball_to_start_carrying = 0.30 - carry_ball_pos = ball_pos - ball_to_goal_dir * dist_from_ball_to_start_carrying + carry_ball_pos = ball_pos - ball_to_target_dir * dist_from_ball_to_start_carrying my_to_ball = ball_pos - my_pos my_to_ball_norm = np.linalg.norm(my_to_ball) @@ -117,15 +135,15 @@ class Agent: else: my_to_ball_dir = my_to_ball / my_to_ball_norm - cosang = np.dot(my_to_ball_dir, ball_to_goal_dir) + cosang = np.dot(my_to_ball_dir, ball_to_target_dir) cosang = np.clip(cosang, -1.0, 1.0) angle_diff = np.arccos(cosang) ANGLE_TOL = np.deg2rad(7.5) aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL) - behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < 0 - desired_orientation = MathOps.vector_angle(ball_to_goal) + behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0 + desired_orientation = MathOps.vector_angle(ball_to_target) if not aligned or not behind_ball: self.agent.skills_manager.execute( @@ -137,8 +155,140 @@ class Agent: else: self.agent.skills_manager.execute( "Walk", - target_2d=their_goal_pos, + target_2d=target_point, is_target_absolute=True, orientation=desired_orientation ) + def kick_ball(self, target_point=None): + if target_point is None: + target_point = self.target_point + + target_point = np.asarray(target_point, dtype=float)[:2] + ball_pos = self.agent.world.ball_pos[:2] + my_pos = self.agent.world.global_position[:2] + + ball_to_target = target_point - ball_pos + ball_to_target_dist = np.linalg.norm(ball_to_target) + if ball_to_target_dist <= 1e-6: + return False + if self.is_kicking: + if self.current_shot_target_point is None: + self.current_shot_target_point = target_point + + active_target = self.current_shot_target_point + active_distance = np.linalg.norm(active_target - ball_pos) + self.is_kicking = not self.agent.skills_manager.execute( + "Kick", distance=active_distance + ) + if not self.is_kicking: + self.current_shot_target_point = None + return True + + my_to_ball = ball_pos - my_pos + my_to_ball_dist = np.linalg.norm(my_to_ball) + + ball_to_target_dir = ball_to_target / ball_to_target_dist + behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0 + me_to_ball_dir = ball_pos - my_pos + me_to_ball_dir_norm = np.linalg.norm(me_to_ball_dir) + + desired_orientation = MathOps.vector_angle(me_to_ball_dir) + current_orientation = self.agent.robot.global_orientation_euler[2] + orientation_error = abs( + MathOps.normalize_deg(desired_orientation - current_orientation) + ) + + lateral_error_to_shot_line, _ = MathOps.distance_point_to_line( + p=my_pos, + a=ball_pos, + b=target_point, + ) + + close_to_ball = my_to_ball_dist <= 0.24 + good_orientation = orientation_error <= 14.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_pos = ball_pos - ball_to_target_dir * prepare_offset + + self.agent.skills_manager.execute( + "Walk", + target_2d=prepare_pos, + is_target_absolute=True, + orientation=desired_orientation, + ) + return True + + self.current_shot_target_point = target_point + self.is_kicking = not self.agent.skills_manager.execute( + "Kick", distance=ball_to_target_dist + ) + if not self.is_kicking: + self.current_shot_target_point = None + return True + + def get_active_player_number(self) -> int: + world = self.agent.world + ball_pos = world.ball_pos[:2] + server_time = world.server_time + + my_num = world.number + my_pos = world.global_position[:2] + best_player_number = my_num + best_distance = np.linalg.norm(my_pos - ball_pos) + + for teammate_number, teammate in enumerate(world.our_team_players, start=1): + if teammate_number == my_num: + continue + + if teammate.last_seen_time is None or server_time is None: + continue + + info_age = server_time - teammate.last_seen_time + if info_age > self.TEAMMATE_INFO_MAX_AGE: + continue + + teammate_pos = teammate.position[:2] + teammate_distance = np.linalg.norm(teammate_pos - ball_pos) + + if teammate_distance + 1e-6 < best_distance: + best_distance = teammate_distance + best_player_number = teammate_number + elif abs(teammate_distance - best_distance) <= 1e-6: + best_player_number = min(best_player_number, teammate_number) + + return best_player_number + + def execute_support_behavior(self) -> None: + world = self.agent.world + my_pos = world.global_position[:2] + ball_pos = world.ball_pos[:2] + target_point = self.target_point + + ball_to_target = target_point - ball_pos + bt_norm = np.linalg.norm(ball_to_target) + if bt_norm <= 1e-6: + ball_to_target_dir = np.array([1.0, 0.0]) + else: + ball_to_target_dir = ball_to_target / bt_norm + + support_pos = ball_pos - ball_to_target_dir * self.SUPPORT_DISTANCE_FROM_BALL + support_orientation = MathOps.vector_angle(ball_pos - my_pos) + + self.agent.skills_manager.execute( + "Walk", + target_2d=support_pos, + is_target_absolute=True, + orientation=support_orientation, + ) diff --git a/behaviors/behavior_manager.py b/behaviors/behavior_manager.py index c54f17c..a5590d2 100644 --- a/behaviors/behavior_manager.py +++ b/behaviors/behavior_manager.py @@ -1,4 +1,5 @@ from behaviors.custom.keyframe.get_up.get_up import GetUp +from behaviors.custom.keyframe.kick.kick import Kick from behaviors.custom.keyframe.keyframe import KeyframeSkill from behaviors.custom.keyframe.poses.neutral.neutral import Neutral from behaviors.behavior import Behavior @@ -22,7 +23,7 @@ class BehaviorManager: Each skill is indexed by its class name. """ - classes: list[type[Behavior]] = [Walk, Neutral, GetUp] + classes: list[type[Behavior]] = [Walk, Neutral, GetUp, Kick] # instantiate each Skill and store in the skills dictionary self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes} diff --git a/behaviors/custom/keyframe/kick/kick.py b/behaviors/custom/keyframe/kick/kick.py new file mode 100644 index 0000000..dac1371 --- /dev/null +++ b/behaviors/custom/keyframe/kick/kick.py @@ -0,0 +1,91 @@ +import os +from numbers import Real + +from behaviors.behavior import Behavior +from behaviors.custom.keyframe.keyframe import KeyframeSkill + + +class Kick(Behavior): + SHORT_MAX_DISTANCE: float = 1.8 + MID_MAX_DISTANCE: float = 4.5 + TEN_METER_MIN_DISTANCE: float = 8.0 + DEFAULT_DISTANCE: float = 3.0 + + def __init__(self, agent): + super().__init__(agent) + + base_dir = os.path.dirname(__file__) + self.short_kick = KeyframeSkill( + agent=agent, + file=os.path.join(base_dir, "kick_short.yaml"), + ) + self.mid_kick = KeyframeSkill( + agent=agent, + file=os.path.join(base_dir, "kick_mid.yaml"), + ) + self.long_kick = KeyframeSkill( + agent=agent, + file=os.path.join(base_dir, "kick_long.yaml"), + ) + self.ten_meter_kick = KeyframeSkill( + agent=agent, + file=os.path.join(base_dir, "kick_10m.yaml"), + ) + + self.current_kick: KeyframeSkill | None = None + self.should_reset_kick: bool = True + self.should_execute_neutral: bool = True + + def execute(self, reset: bool, *args, **kwargs) -> bool: + if reset: + kick_distance = self._extract_kick_distance(*args, **kwargs) + self.current_kick = self._choose_kick_from_distance(kick_distance) + self.should_reset_kick = True + self.should_execute_neutral = True + + if self.current_kick is None: + self.current_kick = self.mid_kick + + if self.should_execute_neutral: + neutral_finished = self.agent.skills_manager.execute( + "Neutral", + ) + self.should_reset_kick = False + if not neutral_finished: + return False + self.should_execute_neutral = False + self.should_reset_kick = True + + has_finished = self.current_kick.execute(reset=self.should_reset_kick) + self.should_reset_kick = False + return has_finished + + def _extract_kick_distance(self, *args, **kwargs) -> float: + distance_candidates = ( + kwargs.get("distance"), + kwargs.get("kick_distance"), + kwargs.get("target_distance"), + kwargs.get("ball_to_target_distance"), + args[0] if args else None, + ) + + for candidate in distance_candidates: + if isinstance(candidate, Real): + return float(max(0.0, candidate)) + + return self.DEFAULT_DISTANCE + + def _choose_kick_from_distance(self, distance: float) -> KeyframeSkill: + if distance <= self.SHORT_MAX_DISTANCE: + return self.short_kick + + if distance <= self.MID_MAX_DISTANCE: + return self.mid_kick + + if distance >= self.TEN_METER_MIN_DISTANCE: + return self.ten_meter_kick + + return self.long_kick + + def is_ready(self, *args) -> bool: + return True diff --git a/behaviors/custom/keyframe/kick/kick_10m.yaml b/behaviors/custom/keyframe/kick/kick_10m.yaml new file mode 100644 index 0000000..ccf5860 --- /dev/null +++ b/behaviors/custom/keyframe/kick/kick_10m.yaml @@ -0,0 +1,187 @@ +symmetry: false +kp: 175 +kd: 1.4 +keyframes: + - delta: 0.00 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 68.000 + Left_Shoulder_Roll: -76.000 + Left_Elbow_Pitch: -62.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 52.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -26.000 + Left_Hip_Roll: 8.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 62.000 + Left_Ankle_Pitch: 12.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -10.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.23 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -70.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 50.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -40.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 2.500 + Left_Knee_Pitch: 64.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -3.000 + Right_Knee_Pitch: -10.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -6.000 + + - delta: 0.26 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -74.000 + Left_Elbow_Yaw: 0.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: -56.000 + Left_Hip_Roll: 12.000 + Left_Hip_Yaw: 5.000 + Left_Knee_Pitch: 34.000 + Left_Ankle_Pitch: 6.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -6.000 + Right_Knee_Pitch: -8.000 + Right_Ankle_Pitch: 5.000 + Right_Ankle_Roll: -7.000 + + - delta: 0.10 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 74.000 + Left_Shoulder_Roll: -86.000 + Left_Elbow_Pitch: -76.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 44.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -66.000 + Left_Hip_Roll: 13.000 + Left_Hip_Yaw: 5.500 + Left_Knee_Pitch: -12.000 + Left_Ankle_Pitch: 4.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 12.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: -2.500 + Right_Knee_Pitch: -6.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -6.000 + kp: 210 + + - delta: 0.23 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -72.000 + Left_Elbow_Yaw: 0.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: -24.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: 46.000 + Left_Ankle_Pitch: 12.500 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -10.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.23 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -68.000 + Left_Elbow_Yaw: 0.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: -18.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 38.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 12.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -6.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.34 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 64.000 + Left_Shoulder_Roll: -72.000 + Left_Elbow_Pitch: -58.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 68.000 + Right_Shoulder_Roll: 45.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: 0.000 + Left_Hip_Roll: 0.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 0.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 0.000 + Right_Hip_Pitch: 0.000 + Right_Hip_Roll: 0.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: 0.000 + Right_Ankle_Pitch: 0.000 + Right_Ankle_Roll: 0.000 + kp: 120 \ No newline at end of file diff --git a/behaviors/custom/keyframe/kick/kick_long.yaml b/behaviors/custom/keyframe/kick/kick_long.yaml new file mode 100644 index 0000000..ef9e800 --- /dev/null +++ b/behaviors/custom/keyframe/kick/kick_long.yaml @@ -0,0 +1,213 @@ +symmetry: false +kp: 160 +kd: 1.3 +keyframes: + - delta: 0.00 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 68.000 + Left_Shoulder_Roll: -76.000 + Left_Elbow_Pitch: -62.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 52.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -18.000 + Left_Hip_Roll: 8.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 38.000 + Left_Ankle_Pitch: 12.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -38.000 + Right_Ankle_Pitch: 10.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.20 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -70.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 50.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -30.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 2.000 + Left_Knee_Pitch: 24.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 26.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -3.000 + Right_Knee_Pitch: -40.000 + Right_Ankle_Pitch: 10.000 + Right_Ankle_Roll: -6.000 + + - delta: 0.21 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -74.000 + Left_Elbow_Yaw: 0.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: -48.000 + Left_Hip_Roll: 12.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: 4.000 + Left_Ankle_Pitch: 4.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -4.000 + Right_Knee_Pitch: -36.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -7.000 + + - delta: 0.12 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 74.000 + Left_Shoulder_Roll: -86.000 + Left_Elbow_Pitch: -76.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 44.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -56.000 + Left_Hip_Roll: 13.000 + Left_Hip_Yaw: 4.000 + Left_Knee_Pitch: -4.000 + Left_Ankle_Pitch: 2.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 22.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: -1.500 + Right_Knee_Pitch: -28.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -6.000 + + - delta: 0.12 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 76.000 + Left_Shoulder_Roll: -88.000 + Left_Elbow_Pitch: -78.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 42.000 + Right_Elbow_Pitch: -62.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -60.000 + Left_Hip_Roll: 14.000 + Left_Hip_Yaw: 5.000 + Left_Knee_Pitch: -8.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 20.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: -2.000 + Right_Knee_Pitch: -24.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -5.000 + kp: 190 + + - delta: 0.21 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -72.000 + Left_Elbow_Yaw: 0.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: -22.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: 16.000 + Left_Ankle_Pitch: 11.500 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -34.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.21 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -68.000 + Left_Elbow_Yaw: 0.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: -18.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 12.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -34.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.31 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 64.000 + Left_Shoulder_Roll: -72.000 + Left_Elbow_Pitch: -58.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 68.000 + Right_Shoulder_Roll: 45.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: 0.000 + Left_Hip_Roll: 0.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 0.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 0.000 + Right_Hip_Pitch: 20.000 + Right_Hip_Roll: -6.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -26.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: 0.000 + kp: 120 diff --git a/behaviors/custom/keyframe/kick/kick_mid.yaml b/behaviors/custom/keyframe/kick/kick_mid.yaml new file mode 100644 index 0000000..13c789a --- /dev/null +++ b/behaviors/custom/keyframe/kick/kick_mid.yaml @@ -0,0 +1,213 @@ +symmetry: false +kp: 150 +kd: 1.2 +keyframes: + - delta: 0.00 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 68.000 + Left_Shoulder_Roll: -76.000 + Left_Elbow_Pitch: -62.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 52.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -16.000 + Left_Hip_Roll: 8.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 36.000 + Left_Ankle_Pitch: 12.000 + Left_Ankle_Roll: 4.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -36.000 + Right_Ankle_Pitch: 9.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.17 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -70.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 50.000 + Right_Elbow_Pitch: -72.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -28.000 + Left_Hip_Roll: 10.000 + Left_Hip_Yaw: 1.500 + Left_Knee_Pitch: 24.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 26.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -2.000 + Right_Knee_Pitch: -38.000 + Right_Ankle_Pitch: 10.000 + Right_Ankle_Roll: -6.000 + + - delta: 0.18 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -74.000 + Left_Elbow_Yaw: 0.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: -44.000 + Left_Hip_Roll: 12.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: 6.000 + Left_Ankle_Pitch: 5.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -10.000 + Right_Hip_Yaw: -4.000 + Right_Knee_Pitch: -36.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -7.000 + + - delta: 0.12 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 74.000 + Left_Shoulder_Roll: -84.000 + Left_Elbow_Pitch: -76.000 + Left_Elbow_Yaw: 0.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: -52.000 + Left_Hip_Roll: 13.000 + Left_Hip_Yaw: 3.500 + Left_Knee_Pitch: -2.000 + Left_Ankle_Pitch: 2.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: -1.000 + Right_Knee_Pitch: -30.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.12 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 76.000 + Left_Shoulder_Roll: -86.000 + Left_Elbow_Pitch: -78.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 72.000 + Right_Shoulder_Roll: 44.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: -58.000 + Left_Hip_Roll: 14.000 + Left_Hip_Yaw: 4.000 + Left_Knee_Pitch: -6.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 22.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: -1.000 + Right_Knee_Pitch: -26.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -6.000 + kp: 180 + + - delta: 0.18 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -72.000 + Left_Elbow_Yaw: 0.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: -20.000 + Left_Hip_Roll: 10.500 + Left_Hip_Yaw: 2.000 + Left_Knee_Pitch: 20.000 + Left_Ankle_Pitch: 11.000 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -34.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.18 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -68.000 + Left_Elbow_Yaw: 0.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_Roll: 10.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 14.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 24.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -34.000 + Right_Ankle_Pitch: 8.000 + Right_Ankle_Roll: -4.000 + + - delta: 0.26 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 64.000 + Left_Shoulder_Roll: -72.000 + Left_Elbow_Pitch: -58.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 68.000 + Right_Shoulder_Roll: 45.000 + Right_Elbow_Pitch: -64.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: 0.000 + Left_Hip_Roll: 0.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 0.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 0.000 + Right_Hip_Pitch: 20.000 + Right_Hip_Roll: -6.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -26.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: 0.000 + kp: 120 diff --git a/behaviors/custom/keyframe/kick/kick_short.yaml b/behaviors/custom/keyframe/kick/kick_short.yaml new file mode 100644 index 0000000..b4a242d --- /dev/null +++ b/behaviors/custom/keyframe/kick/kick_short.yaml @@ -0,0 +1,213 @@ +symmetry: false +kp: 140 +kd: 1.2 +keyframes: + - delta: 0.00 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 68.000 + Left_Shoulder_Roll: -76.000 + Left_Elbow_Pitch: -62.000 + Left_Elbow_Yaw: 0.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_Roll: 8.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 42.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -10.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.16 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 70.000 + Left_Shoulder_Roll: -80.000 + Left_Elbow_Pitch: -70.000 + Left_Elbow_Yaw: 0.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_Roll: 10.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 54.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 4.500 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -9.000 + Right_Hip_Yaw: -1.000 + Right_Knee_Pitch: -10.000 + Right_Ankle_Pitch: 6.000 + Right_Ankle_Roll: -5.500 + + - delta: 0.17 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -74.000 + Left_Elbow_Yaw: 0.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_Roll: 12.000 + Left_Hip_Yaw: 2.000 + Left_Knee_Pitch: 28.000 + Left_Ankle_Pitch: 10.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 14.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: -2.000 + Right_Knee_Pitch: -8.000 + Right_Ankle_Pitch: 5.000 + Right_Ankle_Roll: -7.000 + + - delta: 0.16 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 73.500 + Left_Shoulder_Roll: -84.000 + Left_Elbow_Pitch: -76.000 + Left_Elbow_Yaw: 0.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_Roll: 13.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: -16.000 + Left_Ankle_Pitch: 8.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 12.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: -1.000 + Right_Knee_Pitch: -6.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -5.000 + + - delta: 0.16 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 75.000 + Left_Shoulder_Roll: -86.000 + Left_Elbow_Pitch: -72.000 + Left_Elbow_Yaw: 0.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_Roll: 13.000 + Left_Hip_Yaw: 3.000 + Left_Knee_Pitch: 10.000 + Left_Ankle_Pitch: 6.000 + Left_Ankle_Roll: 5.000 + Right_Hip_Pitch: 12.000 + Right_Hip_Roll: -8.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -6.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -5.000 + kp: 170 + + - delta: 0.18 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 73.500 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -69.000 + Left_Elbow_Yaw: 0.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_Roll: 9.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 38.000 + Left_Ankle_Pitch: 9.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 8.000 + Right_Hip_Roll: -5.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -8.000 + Right_Ankle_Pitch: 5.000 + Right_Ankle_Roll: -3.000 + + - delta: 0.18 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 72.000 + Left_Shoulder_Roll: -82.000 + Left_Elbow_Pitch: -68.000 + Left_Elbow_Yaw: 0.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_Roll: 9.000 + Left_Hip_Yaw: 1.000 + Left_Knee_Pitch: 34.000 + Left_Ankle_Pitch: 9.000 + Left_Ankle_Roll: 3.000 + Right_Hip_Pitch: 8.000 + Right_Hip_Roll: -5.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: -6.000 + Right_Ankle_Pitch: 4.000 + Right_Ankle_Roll: -3.000 + + - delta: 0.24 + motor_positions: + Head_yaw: 0.000 + Head_pitch: 0.000 + Left_Shoulder_Pitch: 65.000 + Left_Shoulder_Roll: -75.000 + Left_Elbow_Pitch: -60.000 + Left_Elbow_Yaw: 0.000 + Right_Shoulder_Pitch: 65.000 + Right_Shoulder_Roll: 45.000 + Right_Elbow_Pitch: -60.000 + Right_Elbow_Yaw: 0.000 + Waist: 0.000 + Left_Hip_Pitch: 0.000 + Left_Hip_Roll: 0.000 + Left_Hip_Yaw: 0.000 + Left_Knee_Pitch: 0.000 + Left_Ankle_Pitch: 0.000 + Left_Ankle_Roll: 0.000 + Right_Hip_Pitch: 0.000 + Right_Hip_Roll: 0.000 + Right_Hip_Yaw: 0.000 + Right_Knee_Pitch: 0.000 + Right_Ankle_Pitch: 0.000 + Right_Ankle_Roll: 0.000 + kp: 120