♻️ Update sim structure

2025-07-18 19:22:59 +02:00
parent 5a6f195f56
commit d3db2b3650
29 changed files with 1224 additions and 206 deletions
@@ -0,0 +1,120 @@
+import math
+import numpy as np
+from typing import TypedDict
+from enum import Enum
+
+from src.robot.kinematics import BodyStateT
+
+
+class GaitType(Enum):
+    TROT_GATE = 0
+    CRAWL_GATE = 1
+
+
+default_offset = {
+    GaitType.TROT_GATE: [0, 0.5, 0.5, 0],
+    GaitType.CRAWL_GATE: [0, 1 / 4, 2 / 4, 3 / 4],
+}
+
+default_stand_frac = {
+    GaitType.TROT_GATE: 3 / 4,
+    GaitType.CRAWL_GATE: 3 / 4,
+}
+
+
+class GaitStateT(TypedDict):
+    step_height: float
+    step_x: float
+    step_z: float
+    step_angle: float
+    step_velocity: float
+    step_depth: float
+    stand_frac: float
+    offset: list[float]
+    gait_type: GaitType
+
+
+length_multipliers = np.array(
+    [-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5, 1.5, 1.4, 1.0])
+height_profile = np.array(
+    [0.0, 0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 1.1, 1.1, 1.1, 0.0, 0.0])
+
+
+def sine_curve(length, angle, height, phase):
+    x, z = length * (1 - 2 * phase) * np.cos(angle), length * \
+        (1 - 2 * phase) * np.sin(angle)
+    y = height * np.cos(np.pi * (x + z) / (2 * length)) if length else 0
+    return np.array([x, z, y])
+
+
+def yaw_arc(feet, current):
+    return (
+        np.pi / 2
+        + np.arctan2(feet[1], feet[0])
+        + np.arctan2(np.linalg.norm(current[:2] - feet[:2]), np.linalg.norm(feet[:2]))
+    )
+
+
+def get_control_points(length, angle, height):
+    x_polar, z_polar = np.cos(angle), np.sin(angle)
+
+    x = length * length_multipliers * x_polar
+    z = length * length_multipliers * z_polar
+    y = height * height_profile
+    return np.stack([x, z, y], axis=1)
+
+
+def bezier_curve(length, angle, height, phase):
+    ctrl = get_control_points(length, angle, height)
+    n = len(ctrl) - 1
+    coeffs = np.array([math.comb(n, i) * (phase**i) *
+                      ((1 - phase) ** (n - i)) for i in range(n + 1)])
+    return np.sum(ctrl * coeffs[:, None], axis=0)
+
+
+class GaitController:
+    def __init__(self, default_position: np.ndarray):
+        self.default_position = default_position
+        self.phase = 0.0
+
+    def step(self, gait: GaitStateT, body: BodyStateT, dt: float):
+        step_x, step_z, angle = gait["step_x"], gait["step_z"], gait["step_angle"]
+        if not any((step_x, step_z, angle)):
+            body["feet"] = body["feet"] + \
+                (self.default_position - body["feet"]) * dt * 10
+            self.phase = 0.0
+            return
+
+        self._advance_phase(dt, gait["step_velocity"])
+
+        stand_fraction = gait["stand_frac"]
+        depth = gait["step_depth"]
+        height = gait["step_height"]
+        offsets = gait["offset"]
+
+        length = np.hypot(step_x, step_z)
+        if step_x < 0:
+            length = -length
+        turn_amplitude = np.arctan2(step_z, length) * 2
+
+        new_feet = np.zeros_like(self.default_position)
+
+        for i, (default_foot, current_foot) in enumerate(zip(self.default_position, body["feet"])):
+            phase = (self.phase + offsets[i]) % 1
+            ph_norm, curve_fn, amp = self._phase_params(
+                phase, stand_fraction, depth, height)
+            delta_pos = curve_fn(length / 2, turn_amplitude, amp, ph_norm)
+            delta_rot = curve_fn(np.rad2deg(angle), yaw_arc(
+                default_foot, current_foot), amp, ph_norm)
+            new_feet[i][:2] = default_foot[:2] + delta_pos + delta_rot
+            # new_feet[i][3] = 1
+
+        body["feet"] = new_feet
+
+    def _advance_phase(self, dt: float, velocity: float):
+        self.phase = (self.phase + dt * velocity) % 1
+
+    def _phase_params(self, phase: float, stand_frac: float, depth: float, height: float):
+        if phase < stand_frac:
+            return phase / stand_frac, sine_curve, -depth
+        return (phase - stand_frac) / (1 - stand_frac), bezier_curve, height
@@ -0,0 +1,130 @@
+import numpy as np
+from typing import TypedDict, List
+
+import config
+
+
+class BodyStateT(TypedDict):
+    omega: float
+    phi: float
+    psi: float
+    xm: float
+    ym: float
+    zm: float
+    feet: List[List[float]]
+    default_feet: List[List[float]]
+    px: float
+    py: float
+    pz: float
+
+
+def rot_x(theta):
+    c = np.cos(theta)
+    s = np.sin(theta)
+    return np.array([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]])
+
+
+def rot_y(theta):
+    c = np.cos(theta)
+    s = np.sin(theta)
+    return np.array([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]])
+
+
+def rot_z(theta):
+    c = np.cos(theta)
+    s = np.sin(theta)
+    return np.array([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
+
+
+def rot(omega, phi, psi):
+    return rot_z(psi) @ rot_y(phi) @ rot_x(omega)
+
+
+def translation(x, y, z):
+    return np.array([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])
+
+
+def transformation(omega, phi, psi, x, y, z):
+    return rot(omega, phi, psi) @ translation(x, y, z)
+
+
+def get_transformation_matrix(body_state):
+    omega, phi, psi = body_state["omega"], body_state["phi"], body_state["psi"]
+    xm, ym, zm = body_state["xm"], body_state["ym"], body_state["zm"]
+
+    return transformation(omega, phi, psi, xm, ym, zm)
+
+
+class Kinematics:
+    def __init__(self, l1, l2, l3, l4, length, width):
+        self.l1 = float(l1)
+        self.l2 = float(l2)
+        self.l3 = float(l3)
+        self.l4 = float(l4)
+        self.length = float(length)
+        self.width = float(width)
+        self.deg2rad = np.pi / 180
+
+        self.mount_offsets = np.array([
+            [self.length / 2, 0,  self.width / 2],
+            [self.length / 2, 0, -self.width / 2],
+            [-self.length / 2, 0,  self.width / 2],
+            [-self.length / 2, 0, -self.width / 2]
+        ])
+
+        self.inv_mount_rot = np.array([
+            [0, 0, -1],
+            [0, 1,  0],
+            [1, 0,  0]
+        ])
+
+    def get_default_feet_pos(self):
+        feet = self.mount_offsets.copy()
+        feet[:, 1] = -1
+        feet[:, 2] += np.array([self.l1, -self.l1,  self.l1, -self.l1])
+        return feet
+
+    def inverse_kinematics(self, body_state):
+        roll, pitch, yaw = np.deg2rad(body_state["omega"]), np.deg2rad(
+            body_state["phi"]), np.deg2rad(body_state["psi"])
+        xm, ym, zm = body_state["xm"], body_state["ym"], body_state["zm"]
+
+        rot = self._rotation_matrix(roll, pitch, yaw)
+        inv_rot = rot.T
+        inv_tr = - \
+            inv_rot @ np.array([xm, ym, zm])
+
+        angles = []
+        for idx, foot_world in enumerate(body_state["feet"]):
+            foot_body = inv_rot @ foot_world + inv_tr
+            foot_local = self.inv_mount_rot @ (foot_body -
+                                               self.mount_offsets[idx])
+            x_local = -foot_local[0] if idx % 2 else foot_local[0]
+            angles.extend(self._leg_ik(x_local, foot_local[1], foot_local[2]))
+        return angles
+
+    def _leg_ik(self, x, y, z):
+        f = np.sqrt(max(0.0, x*x + y*y - self.l1*self.l1))
+        g = f - self.l2
+        h = np.sqrt(g*g + z*z)
+
+        t1 = -np.arctan2(y, x) - np.arctan2(f, -self.l1)
+
+        d = (h*h - self.l3*self.l3 - self.l4*self.l4) / (2*self.l3*self.l4)
+        d = max(-1.0, min(1.0, d))
+
+        t3 = np.arccos(d)
+        t2 = np.arctan2(z, g) - np.arctan2(self.l4*np.sin(t3),
+                                           self.l3 + self.l4*np.cos(t3))
+
+        return t1, t2, t3
+
+    def _rotation_matrix(self, roll, pitch, yaw):
+        cr, sr = np.cos(roll), np.sin(roll)
+        cp, sp = np.cos(pitch), np.sin(pitch)
+        cy, sy = np.cos(yaw), np.sin(yaw)
+        return np.array([
+            [cp*cy,            -cp*sy,            sp],
+            [sr*sp*cy + cy*cr, -sr*sp*sy + cr*cy, -sr*cp],
+            [sr*sy - sp*cr*cy,  sr*cy + sp*sy*cr,  cr*cp]
+        ])