2 Commits

Author SHA1 Message Date
徐学颢
4decbe1938 Revert "add kick behavior with keyframe, but still need to be improved"
This reverts commit 047d3d60c4.
2026-04-18 18:42:53 +08:00
徐学颢
047d3d60c4 add kick behavior with keyframe, but still need to be improved 2026-04-18 18:32:32 +08:00
10 changed files with 36 additions and 1231 deletions

1
.gitignore vendored
View File

@@ -15,6 +15,7 @@ dist/
*.npz *.npz
*.xml *.xml
*.json *.json
*.yaml
*.iml *.iml
*.pyc *.pyc
.TXT .TXT

View File

@@ -49,9 +49,6 @@ class Agent:
} }
} }
TEAMMATE_INFO_MAX_AGE: float = 0.8
SUPPORT_DISTANCE_FROM_BALL: float = 1.2
def __init__(self, agent): def __init__(self, agent):
""" """
Creates a new DecisionMaker linked to the given agent. Creates a new DecisionMaker linked to the given agent.
@@ -63,10 +60,6 @@ class Agent:
self.agent: Base_Agent = agent self.agent: Base_Agent = agent
self.is_getting_up: bool = False 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: def update_current_behavior(self) -> None:
""" """
@@ -92,19 +85,11 @@ class Agent:
self.is_getting_up = not self.agent.skills_manager.execute(skill_name="GetUp") self.is_getting_up = not self.agent.skills_manager.execute(skill_name="GetUp")
elif self.agent.world.playmode is PlayModeEnum.PLAY_ON: elif self.agent.world.playmode is PlayModeEnum.PLAY_ON:
active_player_number = self.get_active_player_number() self.carry_ball()
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): elif self.agent.world.playmode in (PlayModeEnum.BEFORE_KICK_OFF, PlayModeEnum.THEIR_GOAL, PlayModeEnum.OUR_GOAL):
self.agent.skills_manager.execute("Neutral") self.agent.skills_manager.execute("Neutral")
else: else:
# self.execute_support_behavior() self.carry_ball()
self.agent.skills_manager.execute("Neutral")
self.agent.robot.commit_motor_targets_pd() self.agent.robot.commit_motor_targets_pd()
@@ -112,21 +97,18 @@ class Agent:
""" """
Basic example of a behavior: moves the robot toward the goal while handling the ball. Basic example of a behavior: moves the robot toward the goal while handling the ball.
""" """
target_point = self.target_point their_goal_pos = self.agent.world.field.get_their_goal_position()[:2]
ball_pos = self.agent.world.ball_pos[:2] ball_pos = self.agent.world.ball_pos[:2]
my_pos = self.agent.world.global_position[:2] my_pos = self.agent.world.global_position[:2]
if self.kick_ball(target_point=target_point): ball_to_goal = their_goal_pos - ball_pos
bg_norm = np.linalg.norm(ball_to_goal)
if bg_norm == 0:
return 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 dist_from_ball_to_start_carrying = 0.30
carry_ball_pos = ball_pos - ball_to_target_dir * dist_from_ball_to_start_carrying carry_ball_pos = ball_pos - ball_to_goal_dir * dist_from_ball_to_start_carrying
my_to_ball = ball_pos - my_pos my_to_ball = ball_pos - my_pos
my_to_ball_norm = np.linalg.norm(my_to_ball) my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -135,15 +117,15 @@ class Agent:
else: else:
my_to_ball_dir = my_to_ball / my_to_ball_norm my_to_ball_dir = my_to_ball / my_to_ball_norm
cosang = np.dot(my_to_ball_dir, ball_to_target_dir) cosang = np.dot(my_to_ball_dir, ball_to_goal_dir)
cosang = np.clip(cosang, -1.0, 1.0) cosang = np.clip(cosang, -1.0, 1.0)
angle_diff = np.arccos(cosang) angle_diff = np.arccos(cosang)
ANGLE_TOL = np.deg2rad(7.5) ANGLE_TOL = np.deg2rad(7.5)
aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL) aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0 behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < 0
desired_orientation = MathOps.vector_angle(ball_to_target) desired_orientation = MathOps.vector_angle(ball_to_goal)
if not aligned or not behind_ball: if not aligned or not behind_ball:
self.agent.skills_manager.execute( self.agent.skills_manager.execute(
@@ -155,140 +137,8 @@ class Agent:
else: else:
self.agent.skills_manager.execute( self.agent.skills_manager.execute(
"Walk", "Walk",
target_2d=target_point, target_2d=their_goal_pos,
is_target_absolute=True, is_target_absolute=True,
orientation=desired_orientation 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,
)

View File

@@ -1,5 +1,4 @@
from behaviors.custom.keyframe.get_up.get_up import GetUp 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.keyframe import KeyframeSkill
from behaviors.custom.keyframe.poses.neutral.neutral import Neutral from behaviors.custom.keyframe.poses.neutral.neutral import Neutral
from behaviors.behavior import Behavior from behaviors.behavior import Behavior
@@ -23,7 +22,7 @@ class BehaviorManager:
Each skill is indexed by its class name. Each skill is indexed by its class name.
""" """
classes: list[type[Behavior]] = [Walk, Neutral, GetUp, Kick] classes: list[type[Behavior]] = [Walk, Neutral, GetUp]
# instantiate each Skill and store in the skills dictionary # instantiate each Skill and store in the skills dictionary
self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes} self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes}

View File

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

View File

@@ -2,33 +2,6 @@ symmetry: true
kp: 75 kp: 75
kd: 1 kd: 1
keyframes: 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 - delta: 1
motor_positions: motor_positions:
@@ -36,15 +9,15 @@ keyframes:
Head_pitch: 0.000 Head_pitch: 0.000
Shoulder_Pitch: -114.592 Shoulder_Pitch: -114.592
Shoulder_Roll: -71.619 Shoulder_Roll: -71.619
Elbow_Pitch: -100.944 Elbow_Pitch: -85.944
Elbow_Yaw: -171.000 Elbow_Yaw: -171.887
Waist: 0.000 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 kp: 250
kd: 1 kd: 1
p_gains: p_gains:
@@ -58,91 +31,13 @@ keyframes:
Hip_Yaw: 1 Hip_Yaw: 1
Knee_Pitch: 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: motor_positions:
Head_yaw: 0.000 Head_yaw: 0.000
Head_pitch: 0.000 Head_pitch: 0.000
Shoulder_Pitch: -114.592 Shoulder_Pitch: -114.592
Shoulder_Roll: -89.954 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 Elbow_Yaw: 0.000
Waist: 0.000 Waist: 0.000
Hip_Pitch: -45.836 Hip_Pitch: -45.836
@@ -151,38 +46,15 @@ keyframes:
Knee_Pitch: -179.908 Knee_Pitch: -179.908
Ankle_Pitch: 63.026 Ankle_Pitch: 63.026
Ankle_Roll: 45.836 Ankle_Roll: 45.836
kp: 50
kd: 2
p_gains: 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: motor_positions:
Head_yaw: 0.000 Head_yaw: 0.000
Head_pitch: 0.000 Head_pitch: 0.000
@@ -197,11 +69,11 @@ keyframes:
Knee_Pitch: -42.971 Knee_Pitch: -42.971
Ankle_Pitch: 57.296 Ankle_Pitch: 57.296
Ankle_Roll: 0.000 Ankle_Roll: 0.000
kp: 50 kp: 40
kd: 2 kd: 4
p_gains: p_gains:
Ankle_Pitch: 100 Ankle_Pitch: 100
Ankle_Roll: 100 Ankle_Roll: 100
- delta: 0.25 - delta: 0.25
motor_positions: motor_positions:

View File

@@ -1,91 +0,0 @@
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

View File

@@ -1,187 +0,0 @@
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

View File

@@ -1,213 +0,0 @@
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

View File

@@ -1,213 +0,0 @@
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

View File

@@ -1,213 +0,0 @@
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