JackM323
b537677277
Robot into its own Class instead of lose Global Variables that cause circular imports StateClass usage instead of the old RobotState.py New Input Class for Controller and randome intputs
234 lines
7.9 KiB
Python
234 lines
7.9 KiB
Python
"""
|
|
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 |