diff --git a/Controller.py b/Controller.py deleted file mode 100644 index 9f49944..0000000 --- a/Controller.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/Robot.py b/Robot.py new file mode 100644 index 0000000..9c204a0 --- /dev/null +++ b/Robot.py @@ -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() \ No newline at end of file diff --git a/RobotState.py b/RobotState.py deleted file mode 100644 index fb304cd..0000000 --- a/RobotState.py +++ /dev/null @@ -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 \ No newline at end of file diff --git a/RobotState/RobotState.py b/RobotState/RobotState.py deleted file mode 100644 index ba9a4d4..0000000 --- a/RobotState/RobotState.py +++ /dev/null @@ -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 \ No newline at end of file diff --git a/RobotState/idle.py b/RobotState/idle.py deleted file mode 100644 index b0c6ffa..0000000 --- a/RobotState/idle.py +++ /dev/null @@ -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 \ No newline at end of file diff --git a/RobotState/walking.py b/RobotState/walking.py deleted file mode 100644 index b1dc8b0..0000000 --- a/RobotState/walking.py +++ /dev/null @@ -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 diff --git a/inputs/InputProvider.py b/inputs/InputProvider.py new file mode 100644 index 0000000..b9d0fc8 --- /dev/null +++ b/inputs/InputProvider.py @@ -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 \ No newline at end of file diff --git a/inputs/PygameController.py b/inputs/PygameController.py new file mode 100644 index 0000000..df24081 --- /dev/null +++ b/inputs/PygameController.py @@ -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() \ No newline at end of file diff --git a/inputs/RandomeInputProvider.py b/inputs/RandomeInputProvider.py new file mode 100644 index 0000000..297005a --- /dev/null +++ b/inputs/RandomeInputProvider.py @@ -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 \ No newline at end of file diff --git a/kinematics.py b/kinematics.py index 83b99d2..548a22e 100644 --- a/kinematics.py +++ b/kinematics.py @@ -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())) \ No newline at end of file + ikpyInverse(targets, current_rad) \ No newline at end of file diff --git a/main.py b/main.py index 20a58c3..447c373 100644 --- a/main.py +++ b/main.py @@ -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() \ No newline at end of file + # 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() \ No newline at end of file diff --git a/ml/env.py b/ml/env.py index 404bddc..897dac8 100644 --- a/ml/env.py +++ b/ml/env.py @@ -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.""" diff --git a/simulation.py b/simulation.py index db01dc7..5f79bba 100644 --- a/simulation.py +++ b/simulation.py @@ -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 \ No newline at end of file + # 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() \ No newline at end of file diff --git a/states/IdleState.py b/states/IdleState.py new file mode 100644 index 0000000..4dd1176 --- /dev/null +++ b/states/IdleState.py @@ -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 \ No newline at end of file diff --git a/states/State.py b/states/State.py new file mode 100644 index 0000000..7315d4f --- /dev/null +++ b/states/State.py @@ -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] = {} \ No newline at end of file diff --git a/states/WalkingState.py b/states/WalkingState.py new file mode 100644 index 0000000..6119c4a --- /dev/null +++ b/states/WalkingState.py @@ -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 \ No newline at end of file diff --git a/states/WaveState.py b/states/WaveState.py new file mode 100644 index 0000000..f366f6e --- /dev/null +++ b/states/WaveState.py @@ -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" \ No newline at end of file diff --git a/states/__init__.py b/states/__init__.py new file mode 100644 index 0000000..c0b9e4d --- /dev/null +++ b/states/__init__.py @@ -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"] \ No newline at end of file diff --git a/RobotState/ml_walking.py b/states/ml_walking.py similarity index 100% rename from RobotState/ml_walking.py rename to states/ml_walking.py