""" states/WalkingState.py - Walking Gait States (Tripod & Four/Wave) """ from __future__ import annotations from typing import TYPE_CHECKING, Optional import math import numpy as np import config as cfg import DataTypes as dt from states.State import State if TYPE_CHECKING: from Robot import Robot def interpolate_bezier(t: float, p0: float, p1: float, p2: float) -> float: """Quadratic Bezier interpolation.""" return (1.0 - t) ** 2 * p0 + 2.0 * (1.0 - t) * t * p1 + t**2 * p2 class WalkingState(State): """Tripod Gait Walking State.""" def __init__( self, duration: float = cfg.standard_duration, tickpersec: float = cfg.standard_tickpersec, ): self.duration = duration self.tickpersec = tickpersec self.ticks = int(duration * tickpersec) self.current_tick = 0 self.start_pos: Optional[dt.PosArray] = None self.target_pos: Optional[dt.PosArray] = None def enter(self, robot: Robot) -> None: self.current_tick = 0 self.start_pos = dt.PosArray(np.copy(robot.current_pos.data)) self._calculate_target_positions(robot) def _calculate_target_positions(self, robot: Robot) -> None: vx, vy, omega = robot.vector_dirmov vx *= cfg.translation_gain vy *= cfg.translation_gain omega *= cfg.rotation_gain target_temp = [] for i in range(6): cx, cy, cz = robot.center_points[i] # Combine translation and rotation around body origin v_x = vx + (-omega * cy) v_y = vy + (omega * cx) length = (v_x**2 + v_y**2) ** 0.5 if length > 1.0: v_x /= length v_y /= length target_temp.append( [cx + v_x * cfg.step_length, cy + v_y * cfg.step_length, cz] ) self.target_pos = dt.PosArray(target_temp) def execute(self, robot: Robot) -> Optional[str]: # Transition back to idle if velocity is zero and step finished vx, vy, omega = robot.vector_dirmov if ( abs(vx) < 0.01 and abs(vy) < 0.01 and abs(omega) < 0.01 and self.current_tick == 0 ): return "idle" t = self.current_tick / float(self.ticks) tick_pos_temp = [] for leg_id in range(6): if robot.leg_state[leg_id] == "drag": # Linear sliding toward center reference point pos = self.start_pos[leg_id] + ( robot.center_points[leg_id] - self.start_pos[leg_id] ) * t tick_pos_temp.append(pos) elif robot.leg_state[leg_id] == "step": # Bezier curve swing step mid_point = [ (self.start_pos[leg_id][0] + self.target_pos[leg_id][0]) / 2.0, (self.start_pos[leg_id][1] + self.target_pos[leg_id][1]) / 2.0, max(self.start_pos[leg_id][2], self.target_pos[leg_id][2]) + cfg.step_height, ] x = interpolate_bezier( t, self.start_pos[leg_id][0], mid_point[0], self.target_pos[leg_id][0], ) y = interpolate_bezier( t, self.start_pos[leg_id][1], mid_point[1], self.target_pos[leg_id][1], ) z = interpolate_bezier( t, self.start_pos[leg_id][2], mid_point[2], self.target_pos[leg_id][2], ) tick_pos_temp.append([x, y, z]) tick_pos = dt.PosArray(tick_pos_temp) target_rad = robot.compute_ik(tick_pos) # Update robot instance robot.current_pos = tick_pos robot.set_joint_angles(target_rad) self.current_tick += 1 # Gait phase swap when step finishes if self.current_tick > self.ticks: self.current_tick = 0 self.start_pos = dt.PosArray(np.copy(robot.current_pos.data)) if robot.leg_state[0] == "step": robot.leg_state = np.array( ["drag", "step", "drag", "step", "drag", "step"] ) else: robot.leg_state = np.array( ["step", "drag", "step", "drag", "step", "drag"] ) self._calculate_target_positions(robot) return None def exit(self, robot: Robot) -> None: pass class WalkingFourState(State): """4-Leg/Wave Crawl Gait State.""" def __init__( self, duration: float = cfg.standard_duration, tickpersec: float = cfg.standard_tickpersec, ): self.duration = duration self.tickpersec = tickpersec self.ticks = int(duration * tickpersec) self.current_tick = 0 self.start_pos: Optional[dt.PosArray] = None def enter(self, robot: Robot) -> None: self.current_tick = 0 self.start_pos = dt.PosArray(np.copy(robot.current_pos.data)) def execute(self, robot: Robot) -> Optional[str]: t = self.current_tick / float(self.ticks) tick_pos_temp = [] dirmov = robot.vector_dirmov for leg_id in range(6): target_p = [ robot.center_points[leg_id][0] + dirmov[0] * cfg.step_length, robot.center_points[leg_id][1] + dirmov[1] * cfg.step_length, robot.center_points[leg_id][2], ] if robot.leg_state[leg_id] == "drag": tick_pos_temp.append( self.start_pos[leg_id] + (robot.center_points[leg_id] - self.start_pos[leg_id]) * t ) elif robot.leg_state[leg_id] == "step": mid_point = [ (self.start_pos[leg_id][0] + target_p[0]) / 2.0, (self.start_pos[leg_id][1] + target_p[1]) / 2.0, max(self.start_pos[leg_id][2], target_p[2]) + cfg.step_height, ] x = interpolate_bezier( t, self.start_pos[leg_id][0], mid_point[0], target_p[0] ) y = interpolate_bezier( t, self.start_pos[leg_id][1], mid_point[1], target_p[1] ) z = interpolate_bezier( t, self.start_pos[leg_id][2], mid_point[2], target_p[2] ) tick_pos_temp.append([x, y, z]) tick_pos = dt.PosArray(tick_pos_temp) target_rad = robot.compute_ik(tick_pos) robot.current_pos = tick_pos robot.set_joint_angles(target_rad) self.current_tick += 1 if self.current_tick > self.ticks: self.current_tick = 0 self.start_pos = dt.PosArray(np.copy(robot.current_pos.data)) # Rotate wave leg sequence if robot.leg_state[0] == "step": robot.leg_state = np.array( ["drag", "drag", "step", "drag", "drag", "drag"] ) elif robot.leg_state[2] == "step": robot.leg_state = np.array( ["drag", "drag", "drag", "step", "drag", "drag"] ) elif robot.leg_state[3] == "step": robot.leg_state = np.array( ["drag", "drag", "drag", "drag", "drag", "step"] ) elif robot.leg_state[5] == "step": robot.leg_state = np.array( ["step", "drag", "drag", "drag", "drag", "drag"] ) return None def exit(self, robot: Robot) -> None: pass