Compare commits
1 Commits
kick_keyfr
...
get_up
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
c46c0cb314 |
@@ -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.5
|
NEUTRAL_EXECUTION_TIME: float = 1.0
|
||||||
def __init__(self, agent):
|
def __init__(self, agent):
|
||||||
super().__init__(agent)
|
super().__init__(agent)
|
||||||
self.get_up_front = KeyframeSkill(
|
self.get_up_front = KeyframeSkill(
|
||||||
|
|||||||
@@ -2,6 +2,33 @@ 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:
|
||||||
@@ -9,15 +36,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: -85.944
|
Elbow_Pitch: -100.944
|
||||||
Elbow_Yaw: -171.887
|
Elbow_Yaw: -171.000
|
||||||
Waist: 0.000
|
Waist: 0.000
|
||||||
Hip_Pitch: -45.836
|
Hip_Pitch: -35.000
|
||||||
Hip_Roll: 179.908
|
Hip_Roll: 24.000
|
||||||
Hip_Yaw: 179.908
|
Hip_Yaw: 8.000
|
||||||
Knee_Pitch: -179.908
|
Knee_Pitch: -100.000
|
||||||
Ankle_Pitch: 89.954
|
Ankle_Pitch: 70.000
|
||||||
Ankle_Roll: 89.954
|
Ankle_Roll: 10.000
|
||||||
kp: 250
|
kp: 250
|
||||||
kd: 1
|
kd: 1
|
||||||
p_gains:
|
p_gains:
|
||||||
@@ -31,13 +58,91 @@ keyframes:
|
|||||||
Hip_Yaw: 1
|
Hip_Yaw: 1
|
||||||
Knee_Pitch: 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:
|
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: -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
|
Elbow_Yaw: 0.000
|
||||||
Waist: 0.000
|
Waist: 0.000
|
||||||
Hip_Pitch: -45.836
|
Hip_Pitch: -45.836
|
||||||
@@ -46,15 +151,38 @@ 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: 25
|
Hip_Pitch: 120
|
||||||
Hip_Roll: 25
|
Hip_Roll: 120
|
||||||
Hip_Yaw: 25
|
Hip_Yaw: 120
|
||||||
Knee_Pitch: 25
|
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.15
|
||||||
- delta: 0.6
|
|
||||||
motor_positions:
|
motor_positions:
|
||||||
Head_yaw: 0.000
|
Head_yaw: 0.000
|
||||||
Head_pitch: 0.000
|
Head_pitch: 0.000
|
||||||
@@ -69,11 +197,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: 40
|
kp: 50
|
||||||
kd: 4
|
kd: 2
|
||||||
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:
|
||||||
@@ -91,4 +219,4 @@ keyframes:
|
|||||||
Ankle_Pitch: 0.000
|
Ankle_Pitch: 0.000
|
||||||
Ankle_Roll: 0.000
|
Ankle_Roll: 0.000
|
||||||
kp: 200
|
kp: 200
|
||||||
kd: 3.5
|
kd: 3.5
|
||||||
|
|||||||
Reference in New Issue
Block a user