Init

behaviors/custom/keyframe/get_up/get_up.py

@@ -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()

behaviors/custom/keyframe/get_up/get_up_back.yaml

@@ -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

behaviors/custom/keyframe/get_up/get_up_front.yaml

@@ -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

behaviors/custom/keyframe/keyframe.py

@@ -0,0 +1,89 @@
+from behaviors.behavior import Behavior
+import yaml
+
+
+class KeyframeSkill(Behavior):
+    def __init__(self, agent, file: str):
+        super().__init__(agent)
+
+        self.keyframe_step: int = 0
+        self.keyframe_start_time: float = None
+
+        with open(f"{file}", "r") as f:
+            self.skill_description = yaml.safe_load(f)
+
+        self.has_symmetry: bool = self.skill_description["symmetry"]
+        self.keyframes: list = self.skill_description["keyframes"]
+        self.kp: float = self.skill_description["kp"]
+        self.kd: float = self.skill_description["kd"]
+
+    def execute(self, reset, *args, **kwargs):
+        if reset:
+            self.keyframe_step = 0
+            self.keyframe_start_time = self.agent.world.server_time
+
+        current_keyframe = self.keyframes[self.keyframe_step]
+
+        raw_motor_positions = current_keyframe["motor_positions"]
+
+        current_keyframe_kp = current_keyframe.get("kp", self.kp)
+        current_keyframe_kd = current_keyframe.get("kd", self.kd)
+
+        p_gains = current_keyframe.get("p_gains", None)
+        d_gains = current_keyframe.get("d_gains", None)
+
+        for readable_motor_name, position in raw_motor_positions.items():
+
+            p_gain = (
+                p_gains.get(readable_motor_name, current_keyframe_kp)
+                if p_gains
+                else current_keyframe_kp
+            )
+            d_gain = (
+                d_gains.get(readable_motor_name, current_keyframe_kd)
+                if d_gains
+                else current_keyframe_kd
+            )
+
+            if self.has_symmetry:
+                motor_names, is_inverse_direction = self.agent.robot.MOTOR_SYMMETRY[
+                    readable_motor_name
+                ]
+                for motor_name in motor_names:
+                    server_motor_name = self.agent.robot.MOTOR_FROM_READABLE_TO_SERVER[
+                        motor_name
+                    ]
+
+                    self.agent.robot.set_motor_target_position(
+                        motor_name=server_motor_name,
+                        target_position=position if is_inverse_direction else -position,
+                        kp=p_gain,
+                        kd=d_gain,
+                    )
+                    if is_inverse_direction:
+                        is_inverse_direction = False  # Only inverts one joint
+            else:
+                server_motor_name = self.agent.robot.MOTOR_FROM_READABLE_TO_SERVER[
+                    readable_motor_name
+                ]
+                self.agent.robot.set_motor_target_position(
+                    motor_name=server_motor_name,
+                    target_position=position,
+                    kp=self.kp,
+                    kd=self.kd,
+                )
+
+        keyframe_time: float = current_keyframe["delta"]
+        elapsed_keyframe_time = self.agent.world.server_time - self.keyframe_start_time
+
+        if elapsed_keyframe_time >= keyframe_time:
+            self.keyframe_start_time = self.agent.world.server_time
+            self.keyframe_step += 1
+
+            if self.keyframe_step >= len(self.keyframes):
+                return True
+
+        return False
+
+    def is_ready(self, *args):
+        return True

behaviors/custom/keyframe/poses/neutral/neutral.py

@@ -0,0 +1,7 @@
+import os
+from behaviors.custom.keyframe.keyframe import KeyframeSkill
+
+
+class Neutral(KeyframeSkill):
+    def __init__(self, agent):
+        super().__init__(agent, os.path.join(os.path.dirname(__file__), "neutral.yaml"))

behaviors/custom/keyframe/poses/neutral/neutral.yaml

@@ -0,0 +1,19 @@
+symmetry: true
+kp: 150
+kd: 1
+keyframes:
+  - delta: 0.0
+    motor_positions:
+      Head_yaw: 0.000
+      Head_pitch: 0.000
+      Shoulder_Pitch: 0.000
+      Shoulder_Roll: -89.954
+      Elbow_Pitch: 0.000
+      Elbow_Yaw: 0.000
+      Waist: 0.000
+      Hip_Pitch: 0.000
+      Hip_Roll: 0.000
+      Hip_Yaw: 0.000
+      Knee_Pitch: 0.000
+      Ankle_Pitch: 0.000
+      Ankle_Roll: 0.000

behaviors/custom/reinforcement/walk/walk.py

@@ -0,0 +1,147 @@
+import math
+import os
+import numpy as np
+from behaviors.behavior import Behavior
+from utils.math_ops import MathOps
+from utils.neural_network import run_network, load_network
+from scipy.spatial.transform import Rotation as R
+
+class Walk(Behavior):
+    def __init__(self, agent):
+        super().__init__(agent)
+        self.joint_nominal_position = np.array(
+            [
+                0.0,
+                0.0,
+                0.0,
+                1.4,
+                0.0,
+                -0.4,
+                0.0,
+                -1.4,
+                0.0,
+                0.4,
+                0.0,
+                -0.4,
+                0.0,
+                0.0,
+                0.8,
+                -0.4,
+                0.0,
+                0.4,
+                0.0,
+                0.0,
+                -0.8,
+                0.4,
+                0.0,
+            ]
+        )
+        self.train_sim_flip = np.array(
+            [
+                1.0,
+                -1.0,
+                1.0,
+                -1.0,
+                -1.0,
+                1.0,
+                -1.0,
+                -1.0,
+                1.0,
+                1.0,
+                1.0,
+                1.0,
+                -1.0,
+                -1.0,
+                1.0,
+                1.0,
+                -1.0,
+                -1.0,
+                -1.0,
+                -1.0,
+                -1.0,
+                -1.0,
+                -1.0,
+            ]
+        )
+        self.scaling_factor = 0.5
+
+        self.previous_action = np.zeros(len(self.agent.robot.ROBOT_MOTORS))
+
+        self.model = load_network(model_path=os.path.join(os.path.dirname(__file__), "walk.onnx"))
+
+
+    def execute(self, reset: bool, target_2d: list, is_target_absolute: bool, orientation: float=None, is_orientation_absolute: bool=True) -> bool:
+        
+        robot = self.agent.robot
+        world = self.agent.world
+        
+        velocity = None
+
+        if is_target_absolute:
+            raw_target = target_2d - world.global_position[:2]
+            velocity = MathOps.rotate_2d_vec(raw_target, -robot.global_orientation_euler[2], is_rad=False)
+        else:
+            velocity = target_2d
+
+        rel_orientation = None
+        if orientation is None:
+            rel_orientation = MathOps.vector_angle(velocity) * 0.3
+        elif is_orientation_absolute:
+            rel_orientation = MathOps.normalize_deg(orientation - robot.global_orientation_euler[2])
+        else:
+            rel_orientation = orientation * 0.3
+
+        rel_orientation = np.clip(rel_orientation, -0.25, 0.25)
+
+        velocity = np.concatenate([velocity, np.array([rel_orientation])], axis=0)
+
+        velocity[0] = np.clip(velocity[0], -0.5, 0.5)
+        velocity[1] = np.clip(velocity[1], -0.25, 0.25)
+
+        radian_joint_positions = np.deg2rad(list(robot.motor_positions.values()))
+        radian_joint_speeds = np.deg2rad(list(robot.motor_speeds.values()))
+
+        qpos_qvel_previous_action = np.vstack(
+            (
+                [
+                    (
+                        (radian_joint_positions * self.train_sim_flip)
+                        - self.joint_nominal_position
+                    )
+                    / 4.6,
+                    radian_joint_speeds / 110.0 * self.train_sim_flip,
+                    self.previous_action / 10.0,
+                ]
+            )
+        ).T.flatten()
+
+        ang_vel = np.clip(np.deg2rad(robot.gyroscope) / 50.0, -1.0, 1.0)
+        orientation_quat_inv = R.from_quat(robot._global_cheat_orientation).inv()
+        projected_gravity = orientation_quat_inv.apply(np.array([0.0, 0.0, -1.0]))
+        #[0.5,0.25,0.25]
+        observation = np.concatenate(
+            [
+                qpos_qvel_previous_action,
+                ang_vel,
+                velocity,
+                projected_gravity,
+            ]
+        )
+        observation = np.clip(observation, -10.0, 10.0)
+
+        nn_action = run_network(obs=observation, model=self.model)
+
+        target_joint_positions = (
+            self.joint_nominal_position + self.scaling_factor * nn_action
+        )
+        target_joint_positions *= self.train_sim_flip
+
+        self.previous_action = nn_action
+
+        for idx, target in enumerate(target_joint_positions):
+            robot.set_motor_target_position(
+                robot.ROBOT_MOTORS[idx], target*180/math.pi, kp=25, kd=0.6
+            )
+
+    def is_ready(self, *args, **kwargs) -> bool:
+        return True