Apollo3D/Apollo3D_SE
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
ChenXi
2026-03-10 09:35:27 -04:00
commit e31f827726
46 changed files with 2615 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
behaviors/custom/keyframe/get_up/__pycache__/get_up.cpython-311.pyc Normal file
View File

Binary file not shown.

66
behaviors/custom/keyframe/get_up/get_up.py Normal file
View File

@@ -0,0 +1,66 @@
from collections import deque
import logging
import os
from behaviors.behavior import Behavior
from behaviors.custom.keyframe.keyframe import KeyframeSkill
logger = logging.getLogger()
class GetUp(Behavior):
STABILITY_THRESHOLD_CYCLES: int = 3
NEUTRAL_EXECUTION_TIME: float = 1.5
def __init__(self, agent):
super().__init__(agent)
self.get_up_front = KeyframeSkill(
agent=agent,
file=os.path.join(os.path.dirname(__file__), "get_up_front.yaml"),
)
self.get_up_back = KeyframeSkill(
agent=agent,
file=os.path.join(os.path.dirname(__file__), "get_up_back.yaml"),
)
def execute(self, reset, *args, **kwargs):
robot = self.agent.robot
if reset:
self.neutral_start_time = None
self.has_get_up = False
self.gyro_queue = deque(maxlen=self.STABILITY_THRESHOLD_CYCLES)
self.state = 0
self.chosen_get_up = None
self.should_reset_get_up = True
if not self.has_get_up:
if not self.chosen_get_up and self.agent.skills_manager.execute_sub_skill("Neutral", True):
self.gyro_queue.append(max(abs(robot.gyroscope)))
if len(self.gyro_queue) == self.STABILITY_THRESHOLD_CYCLES and all(g < 2.5 for g in self.gyro_queue):
if abs(robot.accelerometer[1]) < 2 and abs(robot.accelerometer[2]) < 3:
if robot.accelerometer[0] < -8:
self.chosen_get_up = self.get_up_front
elif robot.accelerometer[0] > 8:
self.chosen_get_up = self.get_up_back
if self.chosen_get_up:
self.has_get_up = self.chosen_get_up.execute(reset=self.should_reset_get_up)
self.should_reset_get_up = False
else:
if not self.neutral_start_time:
self.neutral_start_time = self.agent.world.server_time
neutral_elapsed_time = (
self.agent.world.server_time - self.neutral_start_time
)
if neutral_elapsed_time < self.NEUTRAL_EXECUTION_TIME:
self.agent.skills_manager.execute_sub_skill(
"Walk", reset=neutral_elapsed_time <= 1e-6, target_2d=(0, 0), is_target_absolute=False
)
else:
return True
return False
def is_ready(self, *args):
return self.agent.world.is_fallen()

94
behaviors/custom/keyframe/get_up/get_up_back.yaml Normal file
View File

@@ -0,0 +1,94 @@
symmetry: true
kp: 75
kd: 1
keyframes:
- delta: 1
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: -114.592
Shoulder_Roll: -71.619
Elbow_Pitch: -85.944
Elbow_Yaw: -171.887
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
kp: 250
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: 0.8125
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: -114.592
Shoulder_Roll: -89.954
Elbow_Pitch: -85.944
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: -45.836
Hip_Roll: 179.908
Hip_Yaw: 179.908
Knee_Pitch: -179.908
Ankle_Pitch: 63.026
Ankle_Roll: 45.836
p_gains:
Hip_Pitch: 25
Hip_Roll: 25
Hip_Yaw: 25
Knee_Pitch: 25
- delta: 0.6
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 89.954
Shoulder_Roll: -89.954
Elbow_Pitch: 0.000
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: 20.054
Hip_Roll: 11.459
Hip_Yaw: 0.000
Knee_Pitch: -42.971
Ankle_Pitch: 57.296
Ankle_Roll: 0.000
kp: 40
kd: 4
p_gains:
Ankle_Pitch: 100
Ankle_Roll: 100
- delta: 0.25
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 0.000
Shoulder_Roll: -77.363
Elbow_Pitch: -85.944
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: 20.054
Hip_Roll: 7.162
Hip_Yaw: 0.000
Knee_Pitch: -42.971
Ankle_Pitch: 0.000
Ankle_Roll: 0.000
kp: 200
kd: 3.5

91
behaviors/custom/keyframe/get_up/get_up_front.yaml Normal file
View File

@@ -0,0 +1,91 @@
symmetry: true
kp: 75
kd: 1
keyframes:
- delta: 0.1
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: -89.954
Shoulder_Roll: -71.619
Elbow_Pitch: 0.000
Elbow_Yaw: -114.592
Waist: 0.000
Hip_Pitch: 0.000
Hip_Roll: 14.324
Hip_Yaw: 0.000
Knee_Pitch: 0.000
Ankle_Pitch: 0.000
Ankle_Roll: 0.000
- delta: 0.5
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 57.296
Shoulder_Roll: -71.619
Elbow_Pitch: -85.944
Elbow_Yaw: -171.887
Waist: 0.000
Hip_Pitch: 0.000
Hip_Roll: 14.324
Hip_Yaw: 0.000
Knee_Pitch: -28.648
Ankle_Pitch: 0.000
Ankle_Roll: 0.000
- delta: 0.8125
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 89.954
Shoulder_Roll: -89.954
Elbow_Pitch: -85.944
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: 200.536
Hip_Roll: 143.239
Hip_Yaw: 114.592
Knee_Pitch: -171.887
Ankle_Pitch: 89.954
Ankle_Roll: 24.919
- delta: 0.45
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 89.954
Shoulder_Roll: -89.954
Elbow_Pitch: -85.944
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: 20.054
Hip_Roll: 11.459
Hip_Yaw: 0.000
Knee_Pitch: -42.972
Ankle_Pitch: 0.000
Ankle_Roll: 0.000
kp: 50
kd: 2
p_gains:
Ankle_Pitch: 50
Ankle_Roll: 50
- delta: 0.13
motor_positions:
Head_yaw: 0.000
Head_pitch: 0.000
Shoulder_Pitch: 0.000
Shoulder_Roll: -77.363
Elbow_Pitch: -85.944
Elbow_Yaw: 0.000
Waist: 0.000
Hip_Pitch: 20.054
Hip_Roll: 7.162
Hip_Yaw: 0.000
Knee_Pitch: -42.972
Ankle_Pitch: 0.000
Ankle_Roll: 0.000
kp: 200
kd: 1.95