JackM323
9c31de3c38
config into dataclass and enums new Gui that includes settings deleted GlobalVariables small fixes (import, names...)
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
|
|
|
|
from config import 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.step_duration,
|
|
tickpersec: float = cfg.tick_rate_hz,
|
|
):
|
|
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.step_duration,
|
|
tickpersec: float = cfg.tick_rate_hz,
|
|
):
|
|
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 |