Complete Restructered Robot Code
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
This commit is contained in:
-200
@@ -1,200 +0,0 @@
|
||||
import pygame
|
||||
import math
|
||||
import GlobalVariables as gv
|
||||
|
||||
def controller():
|
||||
pygame.init()
|
||||
|
||||
# This is a simple class that will help us print to the screen.
|
||||
# It has nothing to do with the joysticks, just outputting the
|
||||
# information.
|
||||
class TextPrint:
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
self.font = pygame.font.Font(None, 25)
|
||||
|
||||
def tprint(self, screen, text):
|
||||
text_bitmap = self.font.render(text, True, (0, 0, 0))
|
||||
screen.blit(text_bitmap, (self.x, self.y))
|
||||
self.y += self.line_height
|
||||
|
||||
def reset(self):
|
||||
self.x = 10
|
||||
self.y = 10
|
||||
self.line_height = 15
|
||||
|
||||
def indent(self):
|
||||
self.x += 10
|
||||
|
||||
def unindent(self):
|
||||
self.x -= 10
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
def normalize_controller_input(x, y):
|
||||
# Berechne die Laenge des Vektors
|
||||
magnitude = math.sqrt(x**2 + y**2)
|
||||
|
||||
# Wenn die Laenge groesser als 1 ist, normalisiere sie
|
||||
if magnitude > 1.0:
|
||||
x /= magnitude
|
||||
y /= magnitude
|
||||
|
||||
return x, y
|
||||
|
||||
# Set the width and height of the screen (width, height), and name the window.
|
||||
screen = pygame.display.set_mode((500, 700))
|
||||
pygame.display.set_caption("Controller Inputs")
|
||||
|
||||
# Used to manage how fast the screen updates.
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# Get ready to print.
|
||||
text_print = TextPrint()
|
||||
|
||||
# This dict can be left as-is, since pygame will generate a
|
||||
# pygame.JOYDEVICEADDED event for every joystick connected
|
||||
# at the start of the program.
|
||||
joysticks = {}
|
||||
|
||||
done = False
|
||||
while not done:
|
||||
# Event processing step.
|
||||
# Possible joystick events: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
|
||||
# JOYBUTTONUP, JOYHATMOTION, JOYDEVICEADDED, JOYDEVICEREMOVED
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
done = True # Flag that we are done so we exit this loop.
|
||||
|
||||
if event.type == pygame.JOYBUTTONDOWN:
|
||||
print("Joystick button pressed.")
|
||||
if event.button == 3 and gv.emote is None:
|
||||
gv.emote = "wave"
|
||||
if event.button == 2 and gv.emote is None:
|
||||
gv.robotCommunication.send_text("Huhrensohn")
|
||||
if event.button == 0:
|
||||
joystick = joysticks[event.instance_id]
|
||||
gv
|
||||
if joystick.rumble(0, 0.7, 500):
|
||||
print(f"Rumble effect played on joystick {event.instance_id}")
|
||||
|
||||
if event.type == pygame.JOYBUTTONUP:
|
||||
print("Joystick button released.")
|
||||
|
||||
# Handle hotplugging
|
||||
if event.type == pygame.JOYDEVICEADDED:
|
||||
# This event will be generated when the program starts for every
|
||||
# joystick, filling up the list without needing to create them manually.
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
joysticks[joy.get_instance_id()] = joy
|
||||
print(f"Joystick {joy.get_instance_id()} connencted")
|
||||
|
||||
if event.type == pygame.JOYDEVICEREMOVED:
|
||||
del joysticks[event.instance_id]
|
||||
print(f"Joystick {event.instance_id} disconnected")
|
||||
|
||||
# Drawing step
|
||||
# First, clear the screen to white. Don't put other drawing commands
|
||||
# above this, or they will be erased with this command.
|
||||
screen.fill((255, 255, 255))
|
||||
text_print.reset()
|
||||
|
||||
# Get count of joysticks.
|
||||
joystick_count = pygame.joystick.get_count()
|
||||
|
||||
text_print.tprint(screen, f"Number of joysticks: {joystick_count}")
|
||||
text_print.indent()
|
||||
|
||||
# For each joystick:
|
||||
for joystick in joysticks.values():
|
||||
jid = joystick.get_instance_id()
|
||||
|
||||
text_print.tprint(screen, f"Joystick {jid}")
|
||||
text_print.indent()
|
||||
|
||||
# Get the name from the OS for the controller/joystick.
|
||||
name = joystick.get_name()
|
||||
text_print.tprint(screen, f"Joystick name: {name}")
|
||||
|
||||
guid = joystick.get_guid()
|
||||
text_print.tprint(screen, f"GUID: {guid}")
|
||||
|
||||
power_level = joystick.get_power_level()
|
||||
text_print.tprint(screen, f"Joystick's power level: {power_level}")
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other. Triggers count as axes.
|
||||
axes = joystick.get_numaxes()
|
||||
text_print.tprint(screen, f"Number of axes: {axes}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(axes):
|
||||
axis = joystick.get_axis(i)
|
||||
gv.vector_dirmov = [axis]
|
||||
text_print.tprint(screen, f"Axis {i} value: {axis:>6.3f}")
|
||||
text_print.unindent()
|
||||
|
||||
# Get Movement Direction Vector
|
||||
# Left stick (translation)
|
||||
vx = joystick.get_axis(1) # forward/back
|
||||
vy = joystick.get_axis(0) # strafe
|
||||
|
||||
vx, vy = normalize_controller_input(vx, vy)
|
||||
|
||||
# Right stick X (rotation) - adjust axis index if needed
|
||||
omega = joystick.get_axis(2)
|
||||
|
||||
# Deadzone for rotation
|
||||
if abs(omega) < 0.2:
|
||||
omega = 0.0
|
||||
|
||||
# Apply axis snapping (only for translation)
|
||||
if 0.8 < vx and -0.2 < vy < 0.2:
|
||||
vx, vy = 1.0, 0.0
|
||||
elif vx < -0.8 and -0.2 < vy < 0.2:
|
||||
vx, vy = -1.0, 0.0
|
||||
elif -0.2 < vx < 0.2 and 0.8 < vy:
|
||||
vx, vy = 0.0, 1.0
|
||||
elif -0.2 < vx < 0.2 and vy < -0.8:
|
||||
vx, vy = 0.0, -1.0
|
||||
|
||||
# Final movement vector
|
||||
gv.vector_dirmov = [vx, vy, omega]
|
||||
|
||||
# Robot state
|
||||
if abs(vx) < 0.2 and abs(vy) < 0.2 and abs(omega) < 0.2:
|
||||
gv.robot_state = "idle"
|
||||
else:
|
||||
gv.robot_state = "walking"
|
||||
|
||||
buttons = joystick.get_numbuttons()
|
||||
text_print.tprint(screen, f"Number of buttons: {buttons}")
|
||||
text_print.indent()
|
||||
|
||||
for i in range(buttons):
|
||||
button = joystick.get_button(i)
|
||||
text_print.tprint(screen, f"Button {i:>2} value: {button}")
|
||||
text_print.unindent()
|
||||
|
||||
hats = joystick.get_numhats()
|
||||
text_print.tprint(screen, f"Number of hats: {hats}")
|
||||
text_print.indent()
|
||||
|
||||
# Hat position. All or nothing for direction, not a float like
|
||||
# get_axis(). Position is a tuple of int values (x, y).
|
||||
for i in range(hats):
|
||||
hat = joystick.get_hat(i)
|
||||
text_print.tprint(screen, f"Hat {i} value: {str(hat)}")
|
||||
text_print.unindent()
|
||||
|
||||
text_print.unindent()
|
||||
|
||||
# Go ahead and update the screen with what we've drawn.
|
||||
pygame.display.flip()
|
||||
|
||||
# Limit to 30 frames per second.
|
||||
clock.tick(30)
|
||||
|
||||
main()
|
||||
pygame.quit()
|
||||
@@ -0,0 +1,173 @@
|
||||
"""
|
||||
robot.py - Unified Robot Class for JackBot
|
||||
Handles state, kinematics, backends (Hardware/Simulation), and motion execution.
|
||||
"""
|
||||
|
||||
from typing import Protocol, Optional
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
from states import STATE_REGISTRY
|
||||
from states.State import State
|
||||
import DataTypes as dt
|
||||
import kinematics as kin
|
||||
import robot_init as ri
|
||||
import config as cfg
|
||||
|
||||
|
||||
class RobotBackend(Protocol):
|
||||
"""Abstraction layer for hardware vs simulation output."""
|
||||
def send_angles(self, rad_array: dt.RadArray) -> None:
|
||||
...
|
||||
def step_simulation(self) -> None:
|
||||
...
|
||||
|
||||
|
||||
class HardwareBackend:
|
||||
"""Backend for physical ESP32 or Arduino robot."""
|
||||
def __init__(self, comm_channel):
|
||||
self.comm_channel = comm_channel
|
||||
|
||||
def send_angles(self, rad_array: dt.RadArray) -> None:
|
||||
if self.comm_channel:
|
||||
self.comm_channel.send_motion(rad_array)
|
||||
|
||||
def step_simulation(self) -> None:
|
||||
pass # Physical hardware steps in real-time
|
||||
|
||||
|
||||
class PyBulletBackend:
|
||||
"""Backend for PyBullet simulation execution."""
|
||||
def __init__(self, sim_instance, body_id: int = 0):
|
||||
self.sim = sim_instance
|
||||
self.body_id = body_id
|
||||
|
||||
def send_angles(self, rad_array: dt.RadArray) -> None:
|
||||
if self.sim:
|
||||
self.sim.updatePos(rad_array)
|
||||
|
||||
def step_simulation(self) -> None:
|
||||
if self.sim:
|
||||
self.sim.step()
|
||||
|
||||
|
||||
class Robot:
|
||||
"""
|
||||
Encapsulates a single JackBot hexapod instance.
|
||||
Maintains joint states, leg positions, kinematics, and backend control.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
backend: Optional[RobotBackend] = None,
|
||||
start_pose: str = "init_deg",
|
||||
urdf_path: str = cfg.urdf_path
|
||||
):
|
||||
# 1. Store configuration & backend FIRST
|
||||
self.backend = backend
|
||||
self.urdf_path = urdf_path
|
||||
|
||||
# 2. Initialize kinematics and position data
|
||||
pose_deg = ri.init_deg if start_pose == "init_deg" else ri.init90_deg
|
||||
self.current_rad: dt.RadArray = pose_deg.to_rad()
|
||||
self.current_pos: dt.PosArray = kin.ikpyForward(self.current_rad)
|
||||
self.center_points: dt.PosArray = (
|
||||
ri.get_center_points() if hasattr(ri, "get_center_points") else ri.center_points
|
||||
)
|
||||
|
||||
# 3. Initialize gait / motion variables
|
||||
self.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
||||
self.robot_state = "idle"
|
||||
self.vector_dirmov = [0.0, 0.0, 0.0] # [vx, vy, omega]
|
||||
|
||||
# 4. Set state and trigger enter() LAST
|
||||
self.current_state_key: str = "idle"
|
||||
self.current_state: State = STATE_REGISTRY["idle"]
|
||||
self.current_state.enter(self)
|
||||
|
||||
def change_state(self, new_state: State) -> None:
|
||||
if self.current_state:
|
||||
self.current_state.exit(self)
|
||||
self.current_state = new_state
|
||||
self.current_state.enter(self)
|
||||
|
||||
def update(self) -> None:
|
||||
if self.current_state:
|
||||
self.current_state.execute(self)
|
||||
self.step_sim()
|
||||
|
||||
|
||||
# -------------------------------------------------------------------------
|
||||
# Core Motion Execution
|
||||
# -------------------------------------------------------------------------
|
||||
|
||||
def set_joint_angles(self, target_rad: dt.RadArray) -> None:
|
||||
"""Applies joint angles to internal state and sends to the active backend."""
|
||||
self.current_rad = target_rad
|
||||
|
||||
if self.backend:
|
||||
self.backend.send_angles(target_rad)
|
||||
|
||||
def step_sim(self) -> None:
|
||||
"""Advances physics simulation step if applicable."""
|
||||
if self.backend:
|
||||
self.backend.step_simulation()
|
||||
|
||||
def reset_to_init(self) -> None:
|
||||
"""Resets the robot to its default standing stance."""
|
||||
self.current_rad = ri.init_deg.to_rad()
|
||||
self.current_pos = kin.ikpyForward(self.current_rad)
|
||||
self.set_joint_angles(self.current_rad)
|
||||
self.step_sim()
|
||||
|
||||
# -------------------------------------------------------------------------
|
||||
# Inverse / Forward Kinematics wrappers bound to this instance
|
||||
# -------------------------------------------------------------------------
|
||||
|
||||
def compute_ik(self, target_pos: dt.PosArray) -> dt.RadArray:
|
||||
"""
|
||||
Computes IK using this specific robot instance's current joint angles
|
||||
as the seed initial guess.
|
||||
"""
|
||||
return kin.ikpyInverse(target_pos, initial_rad=self.current_rad)
|
||||
|
||||
def compute_fk(self, target_rad: Optional[dt.RadArray] = None) -> dt.PosArray:
|
||||
"""Computes Forward Kinematics for joint angles."""
|
||||
rads = target_rad if target_rad is not None else self.current_rad
|
||||
return kin.ikpyForward(rads)
|
||||
|
||||
# -------------------------------------------------------------------------
|
||||
# Machine Learning / RL Helper Methods
|
||||
# -------------------------------------------------------------------------
|
||||
|
||||
def get_observation(self, command: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Returns flat observation vector [joint_angles (18,), command (4,)]
|
||||
for reinforcement learning input.
|
||||
"""
|
||||
joint_flat = self.current_rad.data.flatten()
|
||||
return np.concatenate([joint_flat, command]).astype(np.float32)
|
||||
|
||||
def apply_rl_action(self, action_delta: np.ndarray, scale: float = math.radians(8.0)) -> dt.RadArray:
|
||||
"""
|
||||
Applies continuous angle deltas from an RL policy network.
|
||||
"""
|
||||
current_flat = self.current_rad.data.flatten()
|
||||
new_flat = current_flat + action_delta * scale
|
||||
new_rad = dt.RadArray(new_flat.reshape(6, 3))
|
||||
self.set_joint_angles(new_rad)
|
||||
return new_rad
|
||||
|
||||
def transition_to(self, next_state_key: str) -> None:
|
||||
if next_state_key in STATE_REGISTRY and next_state_key != self.current_state_key:
|
||||
self.current_state.exit(self)
|
||||
self.current_state_key = next_state_key
|
||||
self.current_state = STATE_REGISTRY[next_state_key]
|
||||
self.current_state.enter(self)
|
||||
|
||||
def tick(self) -> None:
|
||||
"""Executes one step of the current active state."""
|
||||
next_state_key = self.current_state.execute(self)
|
||||
if next_state_key:
|
||||
self.transition_to(next_state_key)
|
||||
self.step_sim()
|
||||
-319
@@ -1,319 +0,0 @@
|
||||
import GlobalVariables as gv
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
import kinematics as kin
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
|
||||
# Leg_Pair 1 {leg[num] = 0,2,4}
|
||||
# Leg Pair 2 {leg[num] = 1,3,5}
|
||||
|
||||
def emitCalculation(target_rad: dt.RadArray):
|
||||
if gv.shared_sim != None:
|
||||
gv.shared_sim.updatePos(target_rad)
|
||||
gv.shared_sim.step()
|
||||
if gv.robotCommunication != None:
|
||||
gv.robotCommunication.send_motion(target_rad)
|
||||
gv.current_rad = target_rad
|
||||
|
||||
def initPos():
|
||||
# Init Position and gv.current_rad
|
||||
gv.current_rad = gv.init_deg.to_rad()
|
||||
gv.current_pos = gv.center_points
|
||||
target_rad: dt.RadArray = kin.ikpyInverse(gv.center_points)
|
||||
emitCalculation(target_rad)
|
||||
|
||||
|
||||
def walking(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
|
||||
# init Values
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
tick_pos: dt.PosArray # aktuelle XYZ-Positionen
|
||||
current_pos_copy: dt.PosArray = gv.current_pos
|
||||
vx, vy, omega = gv.vector_dirmov
|
||||
|
||||
# Apply config scaling
|
||||
vx *= cfg.translation_gain
|
||||
vy *= cfg.translation_gain
|
||||
omega *= cfg.rotation_gain
|
||||
|
||||
target_pos_temp = []
|
||||
|
||||
for i in range(6):
|
||||
cx, cy, cz = gv.center_points[i]
|
||||
|
||||
# relative position (assuming body center = 0,0)
|
||||
rx = cx
|
||||
ry = cy
|
||||
|
||||
# rotation component
|
||||
v_rot_x = -omega * ry
|
||||
v_rot_y = omega * rx
|
||||
|
||||
# combine translation + rotation
|
||||
v_x = vx + v_rot_x
|
||||
v_y = vy + v_rot_y
|
||||
|
||||
# normalize combined vector (important!)
|
||||
length = (v_x**2 + v_y**2) ** 0.5
|
||||
if length > 1.0:
|
||||
v_x /= length
|
||||
v_y /= length
|
||||
|
||||
target_pos_temp.append([
|
||||
cx + v_x * cfg.step_length,
|
||||
cy + v_y * cfg.step_length,
|
||||
cz
|
||||
])
|
||||
target_pos: dt.PosArray = target_pos_temp
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
# Smooth Step
|
||||
for tick in range(ticks + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
tick_pos_temp = []
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# lineares Gleiten in die Center-Position
|
||||
tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Mittlerer Kontrollpunkt für Bezier-Kurve
|
||||
mid_point = [
|
||||
(current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
|
||||
(current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
|
||||
max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
|
||||
+ cfg.step_height,
|
||||
]
|
||||
x = interpolate(
|
||||
t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
|
||||
)
|
||||
y = interpolate(
|
||||
t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
|
||||
)
|
||||
z = interpolate(
|
||||
t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
|
||||
)
|
||||
tick_pos_temp.append([x, y, z])
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos_temp)
|
||||
# IK mit letzter Winkelstellung als Startpunkt
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
print("Tick Position: \n", tick_pos)
|
||||
if (cfg.sim == True):
|
||||
gv.shared_sim.step()
|
||||
|
||||
# Timing anpassen, damit Loop gleichmäßig bleibt
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Endposition sichern
|
||||
#emitCalculation(kin.ikpyInverse(target_pos))
|
||||
#print("END TargetPos:\n", target_pos, "\n\n\n")
|
||||
|
||||
# Update current pos
|
||||
gv.current_pos = tick_pos
|
||||
gv.current_rad = kin.ikpyInverse(tick_pos)
|
||||
#emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Gait-Zustand wechseln
|
||||
if gv.leg_state[0] == "step":
|
||||
gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
|
||||
else:
|
||||
gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
||||
|
||||
def walking_four(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
|
||||
# init Values
|
||||
ticks = duration * tickpersec
|
||||
tick_duration = 1 / tickpersec
|
||||
tick_pos: dt.PosArray # aktuelle XYZ-Positionen
|
||||
current_rad_copy: dt.RadArray = gv.current_rad
|
||||
current_pos_copy: dt.PosArray = gv.current_pos
|
||||
dirmov_copy = gv.vector_dirmov
|
||||
|
||||
# Zielpunkte für jede Beinspitze berechnen
|
||||
target_pos_temp = []
|
||||
for i in range(6):
|
||||
target_pos_temp.append(
|
||||
[
|
||||
gv.center_points[i][0] + dirmov_copy[0] * cfg.step_length,
|
||||
gv.center_points[i][1] + dirmov_copy[1] * cfg.step_length,
|
||||
gv.center_points[i][2]
|
||||
]
|
||||
)
|
||||
target_pos: dt.PosArray = target_pos_temp
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
# Smooth Step
|
||||
for tick in range(int(ticks) + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
tick_pos_temp = []
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# lineares Gleiten in die Center-Position
|
||||
tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Mittlerer Kontrollpunkt für Bezier-Kurve
|
||||
mid_point = [
|
||||
(current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
|
||||
(current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
|
||||
max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
|
||||
+ cfg.step_height,
|
||||
]
|
||||
x = interpolate(
|
||||
t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
|
||||
)
|
||||
y = interpolate(
|
||||
t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
|
||||
)
|
||||
z = interpolate(
|
||||
t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
|
||||
)
|
||||
tick_pos_temp.append([x, y, z])
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos_temp)
|
||||
# IK mit letzter Winkelstellung als Startpunkt
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
print("Tick Position: \n", tick_pos)
|
||||
|
||||
# Timing anpassen, damit Loop gleichmäßig bleibt
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Endposition sichern
|
||||
#emitCalculation(kin.ikpyInverse(target_pos))
|
||||
print("END TargetPos:\n", target_pos, "\n\n\n")
|
||||
|
||||
# Gait-Zustand wechseln
|
||||
if gv.leg_state[0] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "step", "drag", "drag", "drag"])
|
||||
elif gv.leg_state[2] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "drag", "step", "drag", "drag"])
|
||||
elif gv.leg_state[3] == "step":
|
||||
gv.leg_state = np.array(["drag", "drag", "drag", "drag", "drag", "step"])
|
||||
elif gv.leg_state[5] == "step":
|
||||
gv.leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
|
||||
|
||||
def wave_emote(cycles=3, duration=1.5, tickpersec=20,
|
||||
height=40, amplitude=25, inward_offset=10):
|
||||
"""
|
||||
Greeting wave using front leg (leg 0)
|
||||
|
||||
Motion:
|
||||
- Z: lifts leg up
|
||||
- Y: waves left/right
|
||||
- X: slightly pulled inward to avoid IK limits
|
||||
"""
|
||||
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
|
||||
# Safe copy
|
||||
base_pos = dt.PosArray(np.copy(gv.current_pos.data))
|
||||
|
||||
leg_id = 0 # front leg
|
||||
|
||||
for cycle in range(cycles):
|
||||
for tick in range(ticks):
|
||||
loop_start = time.perf_counter()
|
||||
|
||||
t = tick / ticks
|
||||
tick_pos = np.copy(base_pos.data)
|
||||
|
||||
cx, cy, cz = base_pos[leg_id]
|
||||
|
||||
# smooth outward-only wave
|
||||
y_wave = math.sin(4 * math.pi * t)
|
||||
y_offset = amplitude * (0.5 * (y_wave + 1))
|
||||
|
||||
# vertical lift
|
||||
z_offset = height * math.sin(math.pi * t)
|
||||
|
||||
# clamp sideways motion
|
||||
max_y_dev = 30
|
||||
new_y = cy + y_offset
|
||||
new_y = max(cy - max_y_dev, min(cy + max_y_dev, new_y))
|
||||
|
||||
tick_pos[leg_id] = [
|
||||
cx + 10, # small forward bias (IMPORTANT)
|
||||
new_y,
|
||||
cz + z_offset
|
||||
]
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos)
|
||||
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Timing
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Return to base pose
|
||||
emitCalculation(kin.ikpyInverse(base_pos))
|
||||
gv.current_pos = base_pos
|
||||
|
||||
def laola_wave_emote(cycles=3, duration=1.5, tickpersec=cfg.standard_tickpersec, height=10, amplitude=30):
|
||||
ticks = int(duration * tickpersec)
|
||||
tick_duration = 1 / tickpersec
|
||||
|
||||
# Safe copy of base position
|
||||
base_pos = dt.PosArray(np.copy(gv.current_pos.data))
|
||||
|
||||
# Legs that perform the wave
|
||||
wave_legs = [1, 4]
|
||||
|
||||
for cycle in range(cycles):
|
||||
for tick in range(ticks):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks # normalized 0 → 1
|
||||
tick_pos = np.copy(base_pos.data)
|
||||
|
||||
for leg_id in wave_legs:
|
||||
cx, cy, cz = base_pos[leg_id]
|
||||
|
||||
# Phase shift for Laola wave
|
||||
phase = 0 if leg_id == 1 else math.pi
|
||||
|
||||
# Sideways motion (Y) — reduced amplitude to avoid overextension
|
||||
y_offset = amplitude * math.sin(2 * math.pi * t + phase)
|
||||
|
||||
# Vertical motion (Z) — full up/down oscillation
|
||||
z_offset = height * math.sin(2 * math.pi * t + phase)
|
||||
|
||||
# Apply movement in YZ plane, X stays fixed
|
||||
tick_pos[leg_id] = [
|
||||
cx,
|
||||
cy + y_offset,
|
||||
cz + z_offset
|
||||
]
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos)
|
||||
|
||||
# IK + send command
|
||||
emitCalculation(kin.ikpyInverse(tick_pos))
|
||||
|
||||
# Timing control
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Return to original stance
|
||||
emitCalculation(kin.ikpyInverse(base_pos))
|
||||
gv.current_pos = base_pos
|
||||
@@ -1,20 +0,0 @@
|
||||
import numpy as np
|
||||
|
||||
class RobotState:
|
||||
def on_enter(self, ctx):
|
||||
pass
|
||||
|
||||
def on_exit(self, ctx):
|
||||
pass
|
||||
|
||||
def update(self, ctx, intent, dt):
|
||||
pass
|
||||
|
||||
class RobotContext:
|
||||
def __init__(self):
|
||||
self.current_rad = None
|
||||
self.current_pos = None
|
||||
self.leg_state = None
|
||||
|
||||
self.robotCommunication = None
|
||||
self.shared_sim = None
|
||||
@@ -1,7 +0,0 @@
|
||||
from RobotState.RobotState import RobotState
|
||||
|
||||
class IdleState(RobotState):
|
||||
def update(self, ctx, intent, dt):
|
||||
if intent.walk:
|
||||
return "walking"
|
||||
return None
|
||||
@@ -1,70 +0,0 @@
|
||||
# RobotState/walking.py
|
||||
import time
|
||||
import numpy as np
|
||||
import config as cfg
|
||||
import kinematics as kin
|
||||
import DataTypes as dt
|
||||
from RobotState.RobotState import RobotState
|
||||
|
||||
class WalkingState(RobotState):
|
||||
|
||||
def on_enter(self, ctx):
|
||||
self.t = 0.0
|
||||
self.tick_duration = 1 / cfg.standard_tickpersec
|
||||
self.ticks = cfg.standard_duration * cfg.standard_tickpersec
|
||||
|
||||
self.current_pos_copy = ctx.current_pos.copy()
|
||||
self.dirmov = [0.0, 0.0]
|
||||
|
||||
def update(self, ctx, intent, dt):
|
||||
# 🛑 Transition check
|
||||
if not intent.walk:
|
||||
return "idle"
|
||||
|
||||
self.dirmov = [intent.move_x, intent.move_y]
|
||||
|
||||
t = self.t / self.ticks
|
||||
tick_pos_temp = []
|
||||
|
||||
for leg_id in range(6):
|
||||
if ctx.leg_state[leg_id] == "drag":
|
||||
tick_pos_temp.append(
|
||||
self.current_pos_copy[leg_id]
|
||||
+ (ctx.center_points[leg_id] - self.current_pos_copy[leg_id]) * t
|
||||
)
|
||||
|
||||
elif ctx.leg_state[leg_id] == "step":
|
||||
mid = [
|
||||
(self.current_pos_copy[leg_id][0] + ctx.center_points[leg_id][0]) / 2,
|
||||
(self.current_pos_copy[leg_id][1] + ctx.center_points[leg_id][1]) / 2,
|
||||
ctx.center_points[leg_id][2] + cfg.step_height,
|
||||
]
|
||||
|
||||
def bez(a, b, c):
|
||||
return (1 - t)**2 * a + 2*(1 - t)*t*b + t*t*c
|
||||
|
||||
x = bez(self.current_pos_copy[leg_id][0], mid[0], ctx.center_points[leg_id][0])
|
||||
y = bez(self.current_pos_copy[leg_id][1], mid[1], ctx.center_points[leg_id][1])
|
||||
z = bez(self.current_pos_copy[leg_id][2], mid[2], ctx.center_points[leg_id][2])
|
||||
|
||||
tick_pos_temp.append([x, y, z])
|
||||
|
||||
tick_pos = dt.PosArray(tick_pos_temp)
|
||||
target_rad = kin.ikpyInverse(tick_pos)
|
||||
|
||||
ctx.current_pos = tick_pos
|
||||
ctx.current_rad = target_rad
|
||||
|
||||
if ctx.robotCommunication:
|
||||
ctx.robotCommunication.send_command(target_rad)
|
||||
|
||||
self.t += 1
|
||||
if self.t >= self.ticks:
|
||||
self.t = 0
|
||||
ctx.leg_state = (
|
||||
np.array(["drag","step","drag","step","drag","step"])
|
||||
if ctx.leg_state[0] == "step"
|
||||
else np.array(["step","drag","step","drag","step","drag"])
|
||||
)
|
||||
|
||||
return None
|
||||
@@ -0,0 +1,27 @@
|
||||
"""
|
||||
inputs/InputProvider.py - Abstraction layer for robot control inputs
|
||||
"""
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass, field
|
||||
from typing import List
|
||||
|
||||
|
||||
@dataclass
|
||||
class CommandFrame:
|
||||
"""Represents a snapshot of control inputs at a single tick."""
|
||||
state: str = "idle"
|
||||
vector_dirmov: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) # [vx, vy, omega]
|
||||
emote: str = ""
|
||||
|
||||
|
||||
class InputProvider(ABC):
|
||||
"""Abstract Base Class for input providers (Joystick, Random/RL, Scripted, etc.)."""
|
||||
|
||||
@abstractmethod
|
||||
def get_command(self) -> CommandFrame:
|
||||
"""Polls or generates the latest input command frame."""
|
||||
pass
|
||||
|
||||
def stop(self) -> None:
|
||||
"""Optional cleanup when shutting down input loop."""
|
||||
pass
|
||||
@@ -0,0 +1,129 @@
|
||||
"""
|
||||
inputs/PygameController.py - Pygame Joystick Input Provider with Corrected Axes
|
||||
"""
|
||||
import math
|
||||
from multiprocessing import Queue
|
||||
import pygame
|
||||
|
||||
from inputs.InputProvider import InputProvider, CommandFrame
|
||||
|
||||
|
||||
class PygameController(InputProvider):
|
||||
def __init__(self, deadzone: float = 0.2):
|
||||
pygame.init()
|
||||
pygame.joystick.init()
|
||||
|
||||
self.screen = pygame.display.set_mode((400, 300))
|
||||
pygame.display.set_caption("JackBot Controller Input")
|
||||
self.font = pygame.font.Font(None, 24)
|
||||
|
||||
self.deadzone = deadzone
|
||||
self.joysticks = {}
|
||||
self.clock = pygame.time.Clock()
|
||||
self.current_command = CommandFrame(state="idle", vector_dirmov=[0.0, 0.0, 0.0])
|
||||
|
||||
def _normalize_input(self, x: float, y: float) -> tuple[float, float]:
|
||||
magnitude = math.sqrt(x**2 + y**2)
|
||||
if magnitude > 1.0:
|
||||
x /= magnitude
|
||||
y /= magnitude
|
||||
return x, y
|
||||
|
||||
def update(self) -> CommandFrame:
|
||||
vx, vy, omega = 0.0, 0.0, 0.0
|
||||
emote = ""
|
||||
|
||||
# Process Pygame events
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pass
|
||||
elif event.type == pygame.JOYDEVICEADDED:
|
||||
joy = pygame.joystick.Joystick(event.device_index)
|
||||
self.joysticks[joy.get_instance_id()] = joy
|
||||
print(f"Joystick {joy.get_instance_id()} connected: {joy.get_name()}")
|
||||
elif event.type == pygame.JOYDEVICEREMOVED:
|
||||
if event.instance_id in self.joysticks:
|
||||
print(f"Joystick {event.instance_id} disconnected")
|
||||
del self.joysticks[event.instance_id]
|
||||
elif event.type == pygame.JOYBUTTONDOWN:
|
||||
if event.button == 3:
|
||||
emote = "wave"
|
||||
|
||||
# Read active controller stick states
|
||||
if self.joysticks:
|
||||
for joystick in self.joysticks.values():
|
||||
# Axis 1 = Left Stick Up/Down (Negated so Forward = +1.0)
|
||||
# Axis 0 = Left Stick Left/Right (Right = +1.0)
|
||||
raw_vx = -joystick.get_axis(1) # Forward (+) / Backward (-)
|
||||
raw_vy = joystick.get_axis(0) # Right (+) / Left (-)
|
||||
|
||||
# Check Axis 3 first for Right Stick X, fallback to Axis 2
|
||||
num_axes = joystick.get_numaxes()
|
||||
raw_omega = joystick.get_axis(3) if num_axes > 3 else joystick.get_axis(2)
|
||||
|
||||
norm_vx, norm_vy = self._normalize_input(raw_vx, raw_vy)
|
||||
|
||||
# Apply deadzones
|
||||
if abs(raw_omega) >= self.deadzone:
|
||||
omega = raw_omega
|
||||
if abs(norm_vx) >= self.deadzone or abs(norm_vy) >= self.deadzone:
|
||||
vx, vy = norm_vx, norm_vy
|
||||
|
||||
# Axis snapping for clean cardinal walking
|
||||
if 0.8 < vx and -0.2 < vy < 0.2:
|
||||
vx, vy = 1.0, 0.0
|
||||
elif vx < -0.8 and -0.2 < vy < 0.2:
|
||||
vx, vy = -1.0, 0.0
|
||||
elif -0.2 < vx < 0.2 and 0.8 < vy:
|
||||
vx, vy = 0.0, 1.0
|
||||
elif -0.2 < vx < 0.2 and vy < -0.8:
|
||||
vx, vy = 0.0, -1.0
|
||||
|
||||
# Determine robot state
|
||||
state = "walking" if (vx != 0.0 or vy != 0.0 or omega != 0.0) else "idle"
|
||||
|
||||
self.current_command = CommandFrame(
|
||||
state=state,
|
||||
vector_dirmov=[vx, vy, omega],
|
||||
emote=emote
|
||||
)
|
||||
|
||||
# Render visual display
|
||||
self.screen.fill((30, 30, 30))
|
||||
|
||||
axis_count = list(self.joysticks.values())[0].get_numaxes() if self.joysticks else 0
|
||||
|
||||
lines = [
|
||||
f"Connected Joysticks: {len(self.joysticks)} (Axes: {axis_count})",
|
||||
f"Robot State: {self.current_command.state.upper()}",
|
||||
f"Vx (Fwd/Bwd): {vx:>6.2f}",
|
||||
f"Vy (Strafe) : {vy:>6.2f}",
|
||||
f"Omega (Yaw) : {omega:>6.2f}",
|
||||
]
|
||||
for i, line in enumerate(lines):
|
||||
txt = self.font.render(line, True, (255, 255, 255))
|
||||
self.screen.blit(txt, (20, 20 + i * 30))
|
||||
|
||||
pygame.display.flip()
|
||||
return self.current_command
|
||||
|
||||
def get_command(self) -> CommandFrame:
|
||||
return self.update()
|
||||
|
||||
def stop(self) -> None:
|
||||
pygame.quit()
|
||||
|
||||
|
||||
def controller_loop(control_queue: Queue):
|
||||
provider = PygameController()
|
||||
try:
|
||||
while True:
|
||||
cmd = provider.get_command()
|
||||
control_queue.put({
|
||||
"state": cmd.state,
|
||||
"dirmov": cmd.vector_dirmov,
|
||||
"emote": cmd.emote
|
||||
})
|
||||
provider.clock.tick(30)
|
||||
finally:
|
||||
provider.stop()
|
||||
@@ -0,0 +1,35 @@
|
||||
"""
|
||||
inputs/RandomInputProvider.py - Random Command Generator for Simulation / Training
|
||||
"""
|
||||
import random
|
||||
import time
|
||||
from inputs.InputProvider import InputProvider, CommandFrame
|
||||
|
||||
|
||||
class RandomInputProvider(InputProvider):
|
||||
def __init__(self, change_interval: float = 2.0):
|
||||
self.change_interval = change_interval
|
||||
self.last_change = time.time()
|
||||
self.current_command = CommandFrame()
|
||||
|
||||
def get_command(self) -> CommandFrame:
|
||||
now = time.time()
|
||||
if now - self.last_change > self.change_interval:
|
||||
self.last_change = now
|
||||
|
||||
# 20% chance to idle, 80% to walk randomly
|
||||
if random.random() < 0.2:
|
||||
self.current_command = CommandFrame(
|
||||
state="idle",
|
||||
vector_dirmov=[0.0, 0.0, 0.0]
|
||||
)
|
||||
else:
|
||||
vx = random.uniform(-1.0, 1.0)
|
||||
vy = random.uniform(-1.0, 1.0)
|
||||
omega = random.uniform(-1.0, 1.0)
|
||||
self.current_command = CommandFrame(
|
||||
state="walking",
|
||||
vector_dirmov=[vx, vy, omega]
|
||||
)
|
||||
|
||||
return self.current_command
|
||||
+32
-152
@@ -1,10 +1,13 @@
|
||||
from ikpy.chain import Chain
|
||||
from ikpy.link import OriginLink, URDFLink
|
||||
from typing import Optional
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
|
||||
import GlobalVariables as gv
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=UserWarning, module="ikpy")
|
||||
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
|
||||
@@ -110,33 +113,38 @@ def ikpyForward(target_rad: dt.RadArray) -> dt.PosArray:
|
||||
target_pos_temp.append([x, y, z])
|
||||
return dt.RadArray(np.array(target_pos_temp))
|
||||
|
||||
def ikpyInverse(target_pos: dt.PosArray, initial_rad: Optional[dt.RadArray] = None) -> dt.RadArray:
|
||||
calculated_rads = []
|
||||
|
||||
def ikpyInverse(target_pos: dt.PosArray) -> dt.RadArray:
|
||||
target_rad_temp = []
|
||||
print("target_pos:\n",target_pos)
|
||||
for leg_id, chain in leg_chains.items():
|
||||
# prepare initial guess
|
||||
guess = np.array([0] + list(gv.current_rad[leg_id]) + [0], dtype=float)
|
||||
for leg_id in range(6):
|
||||
chain = leg_chains[leg_id] # Your IKPy Chain object
|
||||
target_xyz = target_pos[leg_id]
|
||||
|
||||
try:
|
||||
ik_result = chain.inverse_kinematics(
|
||||
target_pos[leg_id],
|
||||
initial_position=guess,
|
||||
max_iter=100
|
||||
full_initial_position = None
|
||||
if initial_rad is not None:
|
||||
# Create a zero array matching the total number of links in the chain
|
||||
full_initial_position = [0.0] * len(chain.links)
|
||||
|
||||
# Map the 3 active joint angles into active link indices
|
||||
active_indices = [i for i, active in enumerate(chain.active_links_mask) if active]
|
||||
|
||||
# Match 3 active joints to the 3 active link positions in the chain
|
||||
for idx, angle in zip(active_indices, initial_rad[leg_id]):
|
||||
full_initial_position[idx] = angle
|
||||
|
||||
if full_initial_position is not None:
|
||||
angles = chain.inverse_kinematics(
|
||||
target_position=target_xyz,
|
||||
initial_position=full_initial_position
|
||||
)
|
||||
except ValueError:
|
||||
# fallback → keep the clipped guess
|
||||
ik_result = guess
|
||||
else:
|
||||
angles = chain.inverse_kinematics(target_position=target_xyz)
|
||||
|
||||
# keep only the 3 actuated joint angles (indices 1,2,3)
|
||||
target_rad_temp.append(ik_result[1:4])
|
||||
# Extract only the active joint angles (3 revolute joints) from IKPy result
|
||||
active_angles = chain.active_from_full(angles)
|
||||
calculated_rads.append(active_angles)
|
||||
|
||||
return dt.RadArray(np.array(target_rad_temp))
|
||||
# for leg_id, chain in leg_chains.items():
|
||||
# ik_result = chain.inverse_kinematics(target_pos[leg_id])
|
||||
# # Remove first element (IKPy adds a "dummy" fixed base joint)
|
||||
# target_rad_temp.append(ik_result[1:4])
|
||||
return dt.RadArray(np.array(target_rad_temp))
|
||||
return dt.RadArray(calculated_rads)
|
||||
|
||||
|
||||
def ikpytest():
|
||||
@@ -161,132 +169,4 @@ def ikpytest():
|
||||
]
|
||||
)
|
||||
current_rad: dt.RadArray = joint_angles_deg.to_rad()
|
||||
ikpyInverse(targets, current_rad)
|
||||
|
||||
|
||||
# walk old
|
||||
"""
|
||||
def walk(duration=standard_duration, ticks=standard_tickrate, curve_height=gv.step_height):
|
||||
tick_duration = duration / ticks
|
||||
tick_positions = np.copy(gv.current_pos)
|
||||
dirmov_copy = gv.vector_dirmov
|
||||
target_pos =[]
|
||||
for i in range(6): # Zielposition berechnen
|
||||
target_pos.append([gv.center_points[i][0] + dirmov_copy[0] * gv.step_length, gv.center_points[i][1] + dirmov_copy[1] * gv.step_length, gv.robot_height ])
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t)**2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
for tick in range(int(ticks) + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# Linear interpolation
|
||||
tick_positions[leg_id] = gv.current_pos[leg_id] + (gv.center_points[leg_id] - gv.current_pos[leg_id]) * t
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Curve movement (Bezier path)
|
||||
help_pos = [
|
||||
target_pos[leg_id][0] - gv.current_pos[leg_id][0],
|
||||
target_pos[leg_id][1] - gv.current_pos[leg_id][1],
|
||||
target_pos[leg_id][2] + curve_height
|
||||
]
|
||||
x = interpolate(t, gv.current_pos[leg_id][0], help_pos[0], target_pos[leg_id][0])
|
||||
y = interpolate(t, gv.current_pos[leg_id][1], help_pos[1], target_pos[leg_id][1])
|
||||
z = interpolate(t, gv.current_pos[leg_id][2], help_pos[2], target_pos[leg_id][2])
|
||||
tick_positions[leg_id] = [x, y, z]
|
||||
|
||||
# Send all legs at once through IK
|
||||
ikpyInverse(tick_positions, gv.current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Final position correction
|
||||
ikpyInverse(target_pos, gv.current_pos)
|
||||
|
||||
if (gv.leg_state[0] == "step"):
|
||||
gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
|
||||
if (gv.leg_state[0] == "drag"):
|
||||
gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
|
||||
# Berechnung der Bein Bewegung - Grade
|
||||
def drag_leg(
|
||||
leg_num,
|
||||
current_pos,
|
||||
target_pos,
|
||||
duration=cfg.standard_duration,
|
||||
ticks=cfg.standard_tickrate,
|
||||
):
|
||||
tick_duration = duration / ticks
|
||||
dragtickvec = (target_pos - current_pos[leg_num]) / ticks
|
||||
|
||||
tick_positions = np.copy(current_pos)
|
||||
|
||||
for tick in range(int(ticks)):
|
||||
start_time = time.perf_counter()
|
||||
|
||||
tick_positions[leg_num] += dragtickvec
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - start_time
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
tick_positions[leg_num] = target_pos
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
# Berechnung der Bein Bewegung - Kurve
|
||||
def curve_leg(
|
||||
leg_num,
|
||||
current_pos,
|
||||
target_pos,
|
||||
curve_height=cfg.step_height,
|
||||
duration=cfg.standard_duration,
|
||||
ticks=cfg.standard_tickrate,
|
||||
):
|
||||
tick_duration = duration / ticks
|
||||
|
||||
help_pos = [
|
||||
target_pos[0] - current_pos[leg_num][0],
|
||||
target_pos[1] - current_pos[leg_num][1],
|
||||
target_pos[2] + curve_height,
|
||||
]
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
tick_positions = np.copy(current_pos)
|
||||
|
||||
for tick in range(int(ticks) + 1):
|
||||
start_time = time.perf_counter()
|
||||
|
||||
t = tick / ticks
|
||||
x = interpolate(t, current_pos[leg_num][0], help_pos[0], target_pos[0])
|
||||
y = interpolate(t, current_pos[leg_num][1], help_pos[1], target_pos[1])
|
||||
z = interpolate(t, current_pos[leg_num][2], help_pos[2], target_pos[2])
|
||||
|
||||
tick_positions[leg_num] = [x, y, z]
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - start_time
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
tick_positions[leg_num] = target_pos
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
time.sleep(0.05)
|
||||
|
||||
"""
|
||||
if __name__ == "__main__":
|
||||
print(ikpyForward(gv.test_deg.to_rad()))
|
||||
ikpyInverse(targets, current_rad)
|
||||
@@ -1,49 +1,51 @@
|
||||
"""
|
||||
main.py - Entry point using object-oriented Robot and Input abstractions
|
||||
"""
|
||||
from threading import Thread
|
||||
from multiprocessing import Process, Manager, Queue
|
||||
from multiprocessing import Queue
|
||||
import time
|
||||
|
||||
# Selfmade Libraries
|
||||
import Controller as ctr
|
||||
import RobotState as rs
|
||||
|
||||
# Global Variables
|
||||
import GlobalVariables as gv
|
||||
import config as cfg
|
||||
from Robot import Robot, HardwareBackend, PyBulletBackend
|
||||
from simulation import Simulation
|
||||
from EspCommunication import ESP32Communication
|
||||
from ArduinoCommunication import ArduinoCommunication
|
||||
from inputs.PygameController import controller_loop
|
||||
|
||||
|
||||
def robot_control():
|
||||
# Connection
|
||||
if gv.robotCommunication != None:
|
||||
gv.robotCommunication.start()
|
||||
def create_robot_instance() -> Robot:
|
||||
if cfg.sim:
|
||||
sim_instance = Simulation()
|
||||
backend = PyBulletBackend(sim_instance)
|
||||
else:
|
||||
comm = ArduinoCommunication() if cfg.arduinoConnection else ESP32Communication()
|
||||
comm.start()
|
||||
backend = HardwareBackend(comm)
|
||||
|
||||
# Start Position
|
||||
rs.initPos()
|
||||
return Robot(backend=backend)
|
||||
|
||||
|
||||
def robot_control_loop(robot: Robot, control_queue: Queue):
|
||||
robot.reset_to_init()
|
||||
time.sleep(1)
|
||||
|
||||
############################## Main Loop ##############################
|
||||
tick = 0.05
|
||||
next_time = time.time()
|
||||
|
||||
while gv.control_pause == False:
|
||||
while True:
|
||||
next_time += tick
|
||||
|
||||
if gv.emote == "wave":
|
||||
rs.initPos()
|
||||
time.sleep(0.3)
|
||||
rs.wave_emote()
|
||||
gv.emote = None
|
||||
gv.robot_state = "idle"
|
||||
continue
|
||||
# Drain the queue to get the latest command frame
|
||||
latest_cmd = None
|
||||
while not control_queue.empty():
|
||||
latest_cmd = control_queue.get_nowait()
|
||||
|
||||
# Robot
|
||||
if gv.robot_state == "idle":
|
||||
rs.initPos()
|
||||
time.sleep(0.2)
|
||||
elif gv.robot_state == "walking":
|
||||
rs.walking()
|
||||
if latest_cmd is not None:
|
||||
robot.robot_state = latest_cmd["state"]
|
||||
robot.vector_dirmov = latest_cmd["dirmov"]
|
||||
|
||||
# Simulation
|
||||
if gv.shared_sim != None:
|
||||
gv.shared_sim.step()
|
||||
# Run state machine tick
|
||||
robot.tick()
|
||||
|
||||
sleep_time = next_time - time.time()
|
||||
if sleep_time > 0:
|
||||
@@ -52,9 +54,14 @@ def robot_control():
|
||||
|
||||
if __name__ == "__main__":
|
||||
controller_queue = Queue()
|
||||
robot_queue = Queue()
|
||||
|
||||
my_robot = create_robot_instance()
|
||||
|
||||
controller_thread = Thread(target=ctr.controller)
|
||||
controller_thread.start()
|
||||
robot_control_thread = Thread(target=robot_control)
|
||||
robot_control_thread.start()
|
||||
# Start Pygame input thread
|
||||
input_thread = Thread(target=controller_loop, args=(controller_queue,), daemon=True)
|
||||
robot_thread = Thread(target=robot_control_loop, args=(my_robot, controller_queue), daemon=True)
|
||||
|
||||
input_thread.start()
|
||||
robot_thread.start()
|
||||
|
||||
robot_thread.join()
|
||||
@@ -1,71 +1,33 @@
|
||||
import math
|
||||
import random
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
# ml/env.py
|
||||
import gymnasium as gym
|
||||
from gymnasium import spaces
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
import config as cfg
|
||||
import robot_init as ri
|
||||
from .sim_manager import SimManager
|
||||
|
||||
from Robot import Robot, PyBulletBackend
|
||||
from ml.sim_manager import SimManager
|
||||
|
||||
class JackBotEnv(gym.Env):
|
||||
"""Gymnasium environment for joint-command learning using SimManager for GUI and simulation control."""
|
||||
|
||||
metadata = {"render_modes": ["human", "rgb_array"]}
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path: str | None = None,
|
||||
use_gui: bool = True,
|
||||
frame_skip: int = 4,
|
||||
random_command: bool = True,
|
||||
max_episode_steps: int = 2000,
|
||||
num_robots: int = 1,
|
||||
robot_spacing: float = 0.5,
|
||||
start_pose: str = "init_deg",
|
||||
):
|
||||
self.urdf_path = urdf_path or cfg.urdf_path
|
||||
self.use_gui = use_gui
|
||||
self.frame_skip = frame_skip
|
||||
self.random_command = random_command
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.num_robots = max(1, num_robots)
|
||||
self.robot_spacing = robot_spacing
|
||||
self.start_pose = start_pose
|
||||
self.action_scale = math.radians(8.0)
|
||||
|
||||
# Delegate physics simulation & GUI management
|
||||
self.sim_manager = SimManager(use_gui=self.use_gui)
|
||||
|
||||
self.robots = []
|
||||
self.plane = None
|
||||
self.robot_joint_indices = []
|
||||
self.joint_lower = None
|
||||
self.joint_upper = None
|
||||
self.initial_angles = None
|
||||
self.commands = None
|
||||
self.step_count = 0
|
||||
self.episode_count = 0
|
||||
self.cumulative_reward = 0.0
|
||||
self.last_action = None
|
||||
self._first_reset = True
|
||||
self._min_steps_before_done = 150
|
||||
|
||||
self._connect_sim()
|
||||
|
||||
total_joints = self.num_robots * len(self.robot_joint_indices[0])
|
||||
observation_dim = total_joints + self.num_robots * 4
|
||||
action_dim = total_joints
|
||||
|
||||
self.observation_space = spaces.Box(
|
||||
low=-np.inf, high=np.inf, shape=(observation_dim,), dtype=np.float32
|
||||
)
|
||||
self.action_space = spaces.Box(
|
||||
low=-1.0, high=1.0, shape=(action_dim,), dtype=np.float32
|
||||
def __init__(self, use_gui: bool = False, num_robots: int = 1):
|
||||
self.sim_manager = SimManager(use_gui=use_gui)
|
||||
self.sim_manager.connect()
|
||||
|
||||
# Load simulation bodies
|
||||
self.plane, self.pb_robots, self.joint_indices = self.sim_manager.load_scene(
|
||||
cfg.urdf_path, num_robots=num_robots
|
||||
)
|
||||
|
||||
# Instantiate dedicated Robot Python object for EACH spawned robot
|
||||
self.robots = [
|
||||
Robot(backend=PyBulletBackend(self.sim_manager, body_id=pb_id))
|
||||
for pb_id in self.pb_robots
|
||||
]
|
||||
|
||||
# Action & Observation Spaces
|
||||
action_dim = num_robots * 18
|
||||
obs_dim = num_robots * (18 + 4)
|
||||
self.action_space = spaces.Box(-1.0, 1.0, shape=(action_dim,), dtype=np.float32)
|
||||
self.observation_space = spaces.Box(-np.inf, np.inf, shape=(obs_dim,), dtype=np.float32)
|
||||
def _robot_base_position(self, robot_id: int, num_robots: int = 1, spacing: float = 0.5) -> list[float]:
|
||||
"""Calculates grid coordinates for spawning multiple robots in PyBullet."""
|
||||
cols = int(math.sqrt(num_robots - 1)) + 1
|
||||
@@ -175,44 +137,24 @@ class JackBotEnv(gym.Env):
|
||||
return np.concatenate([self.joint_angles.flatten(), self.commands.flatten()]).astype(np.float32)
|
||||
|
||||
def step(self, action: np.ndarray):
|
||||
action = np.clip(action, self.action_space.low, self.action_space.high).astype(np.float32)
|
||||
self.last_action = action
|
||||
action_matrix = action.reshape(self.num_robots, -1)
|
||||
self.joint_angles = np.clip(
|
||||
self.joint_angles + action_matrix * self.action_scale,
|
||||
self.joint_lower,
|
||||
self.joint_upper,
|
||||
)
|
||||
action_per_robot = action.reshape(len(self.robots), 18)
|
||||
|
||||
for robot, joint_indices, angles in zip(self.robots, self.robot_joint_indices, self.joint_angles):
|
||||
for joint_index, target_angle in zip(joint_indices, angles):
|
||||
p.setJointMotorControl2(
|
||||
bodyIndex=robot,
|
||||
jointIndex=joint_index,
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=target_angle,
|
||||
force=250,
|
||||
)
|
||||
# Apply RL actions independently to each Robot object instance
|
||||
for robot, act in zip(self.robots, action_per_robot):
|
||||
robot.apply_rl_action(act)
|
||||
|
||||
for _ in range(self.frame_skip):
|
||||
p.stepSimulation()
|
||||
# Step PyBullet physics engine once
|
||||
self.sim_manager.step()
|
||||
|
||||
# Gather observations across all robot objects
|
||||
obs = np.concatenate([
|
||||
robot.get_observation(command=np.zeros(4))
|
||||
for robot in self.robots
|
||||
])
|
||||
|
||||
self.step_count += 1
|
||||
observation = self._get_obs()
|
||||
reward = self._compute_reward()
|
||||
self.cumulative_reward += reward
|
||||
|
||||
done = self._is_done()
|
||||
if self.step_count <= self._min_steps_before_done:
|
||||
done = False
|
||||
|
||||
self._update_gui_hud(reward=reward)
|
||||
|
||||
terminated = done
|
||||
truncated = False
|
||||
info = {"step": self.step_count, "episode_reward": self.cumulative_reward}
|
||||
|
||||
return observation, float(reward), terminated, truncated, info
|
||||
done = False
|
||||
return obs, reward, done, False, {}
|
||||
|
||||
def _update_gui_hud(self, reward: float):
|
||||
"""Passes current state metrics to the SimManager HUD renderer."""
|
||||
|
||||
+27
-17
@@ -1,12 +1,13 @@
|
||||
import pybullet as p
|
||||
"""
|
||||
simulation.py - PyBullet Simulation Interface & Standalone Runner
|
||||
"""
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
import RobotState as rs
|
||||
import GlobalVariables as gv
|
||||
import pybullet as p
|
||||
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
import time
|
||||
import os
|
||||
import math
|
||||
|
||||
|
||||
class Simulation:
|
||||
@@ -54,15 +55,24 @@ class Simulation:
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
count = 0
|
||||
current_rad: dt.RadArray = gv.init_deg.to_rad()
|
||||
gv.shared_sim.updatePos(current_rad)
|
||||
from Robot import Robot, PyBulletBackend
|
||||
|
||||
rs.walking()
|
||||
while True:
|
||||
count = +1
|
||||
gv.shared_sim.step()
|
||||
time.sleep(1 / 240)
|
||||
if count > 50:
|
||||
rs.walking()
|
||||
count = 0
|
||||
# 1. Initialize PyBullet simulation environment
|
||||
sim_instance = Simulation()
|
||||
backend = PyBulletBackend(sim_instance)
|
||||
|
||||
# 2. Instantiate Robot with simulation backend
|
||||
robot = Robot(backend=backend)
|
||||
robot.reset_to_init()
|
||||
|
||||
# 3. Command forward movement [vx, vy, omega]
|
||||
robot.vector_dirmov = [1.0, 0.0, 0.0]
|
||||
|
||||
# 4. Main test execution loop
|
||||
try:
|
||||
while True:
|
||||
# Executes state machine logic (Idle -> Walking -> Target Step -> Physics Step)
|
||||
robot.tick()
|
||||
time.sleep(1.0 / 60.0)
|
||||
except KeyboardInterrupt:
|
||||
sim_instance.disconnect()
|
||||
@@ -0,0 +1,30 @@
|
||||
"""
|
||||
states/IdleState.py - Stance & Idle state
|
||||
"""
|
||||
from __future__ import annotations
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
from states.State import State
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from Robot import Robot
|
||||
|
||||
|
||||
class IdleState(State):
|
||||
def enter(self, robot: "Robot") -> None:
|
||||
robot.reset_to_init()
|
||||
|
||||
def execute(self, robot: "Robot") -> Optional[str]:
|
||||
# Check transition conditions based on vector_dirmov or command flags
|
||||
vx, vy, omega = robot.vector_dirmov
|
||||
if abs(vx) > 0.01 or abs(vy) > 0.01 or abs(omega) > 0.01:
|
||||
return "walking"
|
||||
|
||||
if robot.robot_state == "wave":
|
||||
return "wave_emote"
|
||||
if robot.robot_state == "laola":
|
||||
return "laola_emote"
|
||||
|
||||
return None
|
||||
|
||||
def exit(self, robot: "Robot") -> None:
|
||||
pass
|
||||
@@ -0,0 +1,35 @@
|
||||
"""
|
||||
states/State.py - Abstract base class for state machine
|
||||
"""
|
||||
from abc import ABC, abstractmethod
|
||||
from typing import TYPE_CHECKING, Optional, Dict
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from Robot import Robot
|
||||
|
||||
|
||||
class State(ABC):
|
||||
"""Abstract base class for all robot states."""
|
||||
|
||||
@abstractmethod
|
||||
def enter(self, robot: "Robot") -> None:
|
||||
"""Called once when entering the state."""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def execute(self, robot: "Robot") -> Optional[str]:
|
||||
"""
|
||||
Called every control loop tick.
|
||||
Returns Optional[str] containing the name of the next state if transitioning,
|
||||
or None to stay in the current state.
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def exit(self, robot: "Robot") -> None:
|
||||
"""Called once when exiting the state."""
|
||||
pass
|
||||
|
||||
|
||||
# Registry mapping state key names to state instances
|
||||
STATE_REGISTRY: Dict[str, State] = {}
|
||||
@@ -0,0 +1,234 @@
|
||||
"""
|
||||
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
|
||||
@@ -0,0 +1,140 @@
|
||||
"""
|
||||
RobotState/emotes.py - Expressive Emote States (Wave, Laola Wave)
|
||||
"""
|
||||
from __future__ import annotations
|
||||
from typing import TYPE_CHECKING, Optional
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
from states.State import State
|
||||
if TYPE_CHECKING:
|
||||
from Robot import Robot
|
||||
|
||||
|
||||
class WaveEmoteState(State):
|
||||
"""Front leg wave greeting emote state."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
cycles: int = 3,
|
||||
duration: float = 1.5,
|
||||
tickpersec: float = 20.0,
|
||||
height: float = 40.0,
|
||||
amplitude: float = 25.0,
|
||||
):
|
||||
self.cycles = cycles
|
||||
self.duration = duration
|
||||
self.tickpersec = tickpersec
|
||||
self.ticks = int(duration * tickpersec)
|
||||
self.height = height
|
||||
self.amplitude = amplitude
|
||||
|
||||
self.current_cycle = 0
|
||||
self.current_tick = 0
|
||||
self.base_pos: Optional[dt.PosArray] = None
|
||||
|
||||
def enter(self, robot: Robot) -> None:
|
||||
self.current_cycle = 0
|
||||
self.current_tick = 0
|
||||
self.base_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 = np.copy(self.base_pos.data)
|
||||
leg_id = 0 # Front leg
|
||||
|
||||
cx, cy, cz = self.base_pos[leg_id]
|
||||
|
||||
# Wave trajectory calculation
|
||||
y_wave = math.sin(4.0 * math.pi * t)
|
||||
y_offset = self.amplitude * (0.5 * (y_wave + 1.0))
|
||||
z_offset = self.height * math.sin(math.pi * t)
|
||||
|
||||
max_y_dev = 30.0
|
||||
new_y = cx
|
||||
new_y = max(cy - max_y_dev, min(cy + max_y_dev, cy + y_offset))
|
||||
|
||||
tick_pos[leg_id] = [cx + 10.0, new_y, cz + z_offset]
|
||||
|
||||
pos_array = dt.PosArray(tick_pos)
|
||||
target_rad = robot.compute_ik(pos_array)
|
||||
|
||||
robot.current_pos = pos_array
|
||||
robot.set_joint_angles(target_rad)
|
||||
|
||||
self.current_tick += 1
|
||||
|
||||
if self.current_tick >= self.ticks:
|
||||
self.current_tick = 0
|
||||
self.current_cycle += 1
|
||||
|
||||
if self.current_cycle >= self.cycles:
|
||||
return "idle"
|
||||
|
||||
return None
|
||||
|
||||
def exit(self, robot: Robot) -> None:
|
||||
robot.robot_state = "idle"
|
||||
|
||||
|
||||
class LaolaWaveEmoteState(State):
|
||||
"""Laola Wave side-to-side leg wave emote state."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
cycles: int = 3,
|
||||
duration: float = 1.5,
|
||||
tickpersec: float = cfg.standard_tickpersec,
|
||||
height: float = 10.0,
|
||||
amplitude: float = 30.0,
|
||||
):
|
||||
self.cycles = cycles
|
||||
self.duration = duration
|
||||
self.ticks = int(duration * tickpersec)
|
||||
self.height = height
|
||||
self.amplitude = amplitude
|
||||
|
||||
self.current_cycle = 0
|
||||
self.current_tick = 0
|
||||
self.base_pos: Optional[dt.PosArray] = None
|
||||
|
||||
def enter(self, robot: Robot) -> None:
|
||||
self.current_cycle = 0
|
||||
self.current_tick = 0
|
||||
self.base_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 = np.copy(self.base_pos.data)
|
||||
wave_legs = [1, 4]
|
||||
|
||||
for leg_id in wave_legs:
|
||||
cx, cy, cz = self.base_pos[leg_id]
|
||||
phase = 0.0 if leg_id == 1 else math.pi
|
||||
|
||||
y_offset = self.amplitude * math.sin(2.0 * math.pi * t + phase)
|
||||
z_offset = self.height * math.sin(2.0 * math.pi * t + phase)
|
||||
|
||||
tick_pos[leg_id] = [cx, cy + y_offset, cz + z_offset]
|
||||
|
||||
pos_array = dt.PosArray(tick_pos)
|
||||
target_rad = robot.compute_ik(pos_array)
|
||||
|
||||
robot.current_pos = pos_array
|
||||
robot.set_joint_angles(target_rad)
|
||||
|
||||
self.current_tick += 1
|
||||
|
||||
if self.current_tick >= self.ticks:
|
||||
self.current_tick = 0
|
||||
self.current_cycle += 1
|
||||
|
||||
if self.current_cycle >= self.cycles:
|
||||
return "idle"
|
||||
|
||||
return None
|
||||
|
||||
def exit(self, robot: Robot) -> None:
|
||||
robot.robot_state = "idle"
|
||||
@@ -0,0 +1,13 @@
|
||||
"""
|
||||
states/__init__.py - State Machine Initialization
|
||||
"""
|
||||
from states.State import State, STATE_REGISTRY
|
||||
from states.IdleState import IdleState
|
||||
from states.WalkingState import WalkingState, WalkingFourState
|
||||
|
||||
# Register available state instances
|
||||
STATE_REGISTRY["idle"] = IdleState()
|
||||
STATE_REGISTRY["walking"] = WalkingState()
|
||||
STATE_REGISTRY["walking_four"] = WalkingFourState()
|
||||
|
||||
__all__ = ["State", "STATE_REGISTRY", "IdleState", "WalkingState", "WalkingFourState"]
|
||||
Reference in New Issue
Block a user