diff --git a/behaviors/custom/keyframe/get_up/get_up.py b/behaviors/custom/keyframe/get_up/get_up.py index c0be1ef..ee747b2 100644 --- a/behaviors/custom/keyframe/get_up/get_up.py +++ b/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.5 + NEUTRAL_EXECUTION_TIME: float = 1.0 def __init__(self, agent): super().__init__(agent) self.get_up_front = KeyframeSkill( diff --git a/behaviors/custom/keyframe/get_up/get_up_back.yaml b/behaviors/custom/keyframe/get_up/get_up_back.yaml index 5eaae15..306e731 100644 --- a/behaviors/custom/keyframe/get_up/get_up_back.yaml +++ b/behaviors/custom/keyframe/get_up/get_up_back.yaml @@ -2,6 +2,33 @@ 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: @@ -9,15 +36,15 @@ keyframes: Head_pitch: 0.000 Shoulder_Pitch: -114.592 Shoulder_Roll: -71.619 - Elbow_Pitch: -85.944 - Elbow_Yaw: -171.887 + Elbow_Pitch: -100.944 + Elbow_Yaw: -171.000 Waist: 0.000 - Hip_Pitch: -45.836 - Hip_Roll: 179.908 - Hip_Yaw: 179.908 - Knee_Pitch: -179.908 - Ankle_Pitch: 89.954 - Ankle_Roll: 89.954 + Hip_Pitch: -35.000 + Hip_Roll: 24.000 + Hip_Yaw: 8.000 + Knee_Pitch: -100.000 + Ankle_Pitch: 70.000 + Ankle_Roll: 10.000 kp: 250 kd: 1 p_gains: @@ -31,13 +58,91 @@ keyframes: Hip_Yaw: 1 Knee_Pitch: 1 - - delta: 0.8125 + - delta: 0.45 + 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: -85.944 + Elbow_Pitch: -90.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 @@ -46,15 +151,38 @@ keyframes: Knee_Pitch: -179.908 Ankle_Pitch: 63.026 Ankle_Roll: 45.836 + kp: 50 + kd: 2 p_gains: - Hip_Pitch: 25 - Hip_Roll: 25 - Hip_Yaw: 25 - Knee_Pitch: 25 + Hip_Pitch: 120 + Hip_Roll: 120 + Hip_Yaw: 120 + Knee_Pitch: 180 + 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.6 + - delta: 0.15 motor_positions: Head_yaw: 0.000 Head_pitch: 0.000 @@ -69,11 +197,11 @@ keyframes: Knee_Pitch: -42.971 Ankle_Pitch: 57.296 Ankle_Roll: 0.000 - kp: 40 - kd: 4 + kp: 50 + kd: 2 p_gains: - Ankle_Pitch: 100 - Ankle_Roll: 100 + Ankle_Pitch: 100 + Ankle_Roll: 100 - delta: 0.25 motor_positions: @@ -91,4 +219,4 @@ keyframes: Ankle_Pitch: 0.000 Ankle_Roll: 0.000 kp: 200 - kd: 3.5 \ No newline at end of file + kd: 3.5