Project Initialization

First Upload to Gitea
2026-07-29 19:23:45 +02:00
commit c34449aac5
23 changed files with 2046 additions and 0 deletions
@@ -0,0 +1,12 @@
+# Ignore dependency folders
+node_modules/
+.venv/
+.vs/
+__pycache__/
+
+# Ignore environment files with private passwords/keys
+.env
+
+# Ignore OS junk
+.DS_Store
+Thumbs.db
@@ -0,0 +1,74 @@
+from threading import Thread, Event
+from queue import Queue
+import numpy as np
+import serial
+import time
+
+import config as cfg
+import DataTypes as dt
+
+class ArduinoCommunication(Thread):
+    def __init__(self, port=cfg.port, baudrate=cfg.baudrate, timeout=cfg.timeout):
+        super().__init__()
+        self.daemon = True  # Thread schließt sich beim Programmende
+        self.serial_conn  = serial.Serial(port, baudrate, timeout=timeout)
+        time.sleep(2)  # Warten bis Arduino ready
+
+        self.command_queue = Queue()
+        self.response_queue = Queue()
+        self.running = Event()
+        self.running.set()
+
+    def run(self):
+        while self.running.is_set():
+            # 1. Befehle senden
+            if not self.command_queue.empty():
+                command = self.command_queue.get()
+                self._write(command)
+
+            # 2. Antwort lesen (falls vorhanden)
+            if self.serial_conn.in_waiting > 0:
+                line = self.serial_conn.readline().decode('utf-8', errors='ignore').strip()
+                if line:
+                    print(f"[Arduino] {line}")
+                    self.response_queue.put(line)
+
+            time.sleep(0.01)
+
+    # Werte auf Arduino schreiben
+    def write(self, target):
+        if isinstance(target, dt.RadArray):
+            target = dt.DegArray(np.degrees(target.data))
+        if self.serial_conn and self.serial_conn.is_open:
+            leg_str = '\n'.join([','.join(map(str, row)) for row in target])
+            formatted_data = f"<{leg_str}>"
+            print(f"Gesendete Daten:\n{formatted_data}")
+            self.serial_conn.write(bytes(formatted_data, 'utf-8'))
+    
+    # Werte von Arduino ablesen und Synchronisierunga
+    def wait4arduino(self, expected_message):
+        while True:
+            # Lese eine Zeile von der seriellen Verbindung
+            data = self.serial_conn .readline()
+        
+            # Konvertiere die Zeile in einen lesbaren String
+            data = str(data, "utf-8").strip("\r\n")
+        
+            # Debugging-Ausgabe (optional)
+            print(f"Empfangene Daten: {data}")
+        
+            # Ueberpruefe, ob die empfangene Nachricht der erwarteten Nachricht entspricht
+            if data == expected_message:
+                print("Bewegung abgeschlossen!")
+                break
+            else:
+                time.sleep(0.05) 
+
+    def _write(self, message):
+        if self.serial_conn and self.serial_conn.is_open:
+            self.serial_conn.write(message.encode('utf-8'))
+
+    def stop(self):
+        self.running.clear()
+        self.join()
+        self.serial_conn.close()
@@ -0,0 +1,93 @@
+import pygame
+import math
+import time
+
+import DataTypes as dt
+
+def normalize(x, y, deadzone=0.15):
+    if abs(x) < deadzone:
+        x = 0.0
+    if abs(y) < deadzone:
+        y = 0.0
+
+    mag = math.hypot(x, y)
+    if mag > 1.0:
+        x /= mag
+        y /= mag
+
+    return x, y
+
+
+def controller_loop(shared_intent: dt.ControlIntent, lock):
+    pygame.init()
+    pygame.joystick.init()
+
+    joysticks = {}
+    prev_buttons = {}
+
+    clock = pygame.time.Clock()
+
+    while True:
+        # 🔑 REQUIRED: keeps joystick state updating
+        pygame.event.pump()
+
+        for event in pygame.event.get():
+            if event.type == pygame.JOYDEVICEADDED:
+                joy = pygame.joystick.Joystick(event.device_index)
+                joysticks[joy.get_instance_id()] = joy
+                prev_buttons[joy.get_instance_id()] = [0] * joy.get_numbuttons()
+                print(f"[Controller] Joystick connected: {joy.get_name()}")
+
+            elif event.type == pygame.JOYDEVICEREMOVED:
+                joysticks.pop(event.instance_id, None)
+                prev_buttons.pop(event.instance_id, None)
+                print("[Controller] Joystick disconnected")
+
+        if not joysticks:
+            time.sleep(0.1)
+            continue
+
+        # Use first joystick
+        joy = next(iter(joysticks.values()))
+        jid = joy.get_instance_id()
+
+        # ---- AXES ----
+        forward = -joy.get_axis(1)   # usually inverted
+        sideways = joy.get_axis(0)
+
+        forward, sideways = normalize(forward, sideways)
+
+        walk = abs(forward) > 0 or abs(sideways) > 0
+
+        # ---- BUTTONS ----
+        buttons = joy.get_numbuttons()
+        current_buttons = [joy.get_button(i) for i in range(buttons)]
+
+        emote = None
+        quit_flag = False
+
+        for i in range(buttons):
+            if current_buttons[i] and not prev_buttons[jid][i]:
+                # Button DOWN (edge)
+                if i == 0:
+                    emote = "HELLO"
+                elif i == 1:
+                    emote = "SAD"
+                elif i == 9:
+                    quit_flag = True
+
+        prev_buttons[jid] = current_buttons
+
+        # ---- UPDATE INTENT (atomic) ----
+        with lock:
+            shared_intent.move_x = forward
+            shared_intent.move_y = sideways
+            shared_intent.walk = walk
+
+            if emote:
+                shared_intent.emote = emote
+
+            if quit_flag:
+                shared_intent.quit = True
+
+        clock.tick(60)
@@ -0,0 +1,71 @@
+from dataclasses import dataclass
+import numpy as np
+from pygame import Vector2
+
+def checkDimensions(self, row:int, column:int):
+    if self.data.shape != (row, column):
+        raise ValueError(f"{type(self).__name__} must be of shape ({row}, {column}), got {self.data.shape}")
+
+# Dataclasses
+@dataclass
+class PosArray:
+    data: np.ndarray
+    def __post_init__(self):
+        if not isinstance(self.data, np.ndarray):
+            self.data = np.array(self.data, dtype=float)
+        checkDimensions(self, 6, 3)
+    # behave like numpy
+    def __array__(self): return self.data
+    def __getitem__(self, key): return self.data[key]
+    def __iter__(self): return iter(self.data)
+    def __repr__(self): return f"PosArray(\n{self.data}\n)"
+
+
+@dataclass
+class DegArray:
+    data: np.ndarray
+    def __post_init__(self):
+        if not isinstance(self.data, np.ndarray):
+            self.data = np.array(self.data)
+        checkDimensions(self, 6, 3)
+        self.data = self.data.astype(int)
+
+    def to_rad(self) -> "RadArray":
+        return RadArray(np.radians(self.data))
+    # behave like numpy
+    def __array__(self): return self.data
+    def __getitem__(self, key): return self.data[key]
+    def __iter__(self): return iter(self.data)
+    def __repr__(self): return f"PosArray(\n{self.data}\n)"
+
+@dataclass
+class RadArray:
+    data: np.ndarray
+    def __post_init__(self):
+        if not isinstance(self.data, np.ndarray):
+            self.data = np.array(self.data, dtype=float)
+        checkDimensions(self, 6, 3)
+
+    def to_deg(self) -> "DegArray":
+        return DegArray(np.degrees(self.data))
+    # behave like numpy
+    def __array__(self): return self.data
+    def __getitem__(self, key): return self.data[key]
+    def __iter__(self): return iter(self.data)
+    def __repr__(self): return f"PosArray(\n{self.data}\n)"
+
+@dataclass
+class RobotCommand:
+    legs: DegArray
+    state: int = 0
+    text: str = ""
+
+@dataclass
+class ControlIntent:
+    move_x: float = 0.0
+    move_y: float = 0.0
+    turn: float = 0.0
+    emote: str | None = None
+    walk: bool = False
+    quit: bool = False
+
@@ -0,0 +1,2 @@
+[InternetShortcut]
+URL=https://www.thingiverse.com/thing:6345316
@@ -0,0 +1,80 @@
+import socket
+import struct
+
+PACKET_VERSION = 1
+FLOAT_COUNT = 30 * 6 * 3
+
+PACKET_FMT = f"<H I B B {FLOAT_COUNT}f"
+PACKET_SIZE = struct.calcsize(PACKET_FMT)
+
+import time
+import numpy as np
+from threading import Thread, Event
+from queue import Queue
+
+import config as cfg
+import DataTypes as dt
+
+
+class ESP32Communication(Thread):
+    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
+        super().__init__()
+        self.daemon = True
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.sock.settimeout(timeout)
+
+        print(f"Connecting to ESP32 at {host}:{port} ...")
+        self.sock.connect((host, port))
+        print("Connected to ESP32!")
+
+        self.command_queue = Queue()
+        self.response_queue = Queue()
+        self.running = Event()
+        self.running.set()
+
+    def run(self):
+        while self.running.is_set():
+            # 1. Send queued commands
+            if not self.command_queue.empty():
+                command = self.command_queue.get()
+                self._write(command)
+
+            # 2. Read response
+            self.sock.setblocking(False)
+            try:
+                data = self.sock.recv(1024)
+                if data:
+                    line = data.decode('utf-8', errors='ignore').strip()
+                    print(f"[ESP32] {line}")
+                    self.response_queue.put(line)
+            except BlockingIOError:
+                pass
+
+            time.sleep(0.01)
+
+    def write(self, target):
+        if isinstance(target, dt.RadArray):
+            target = dt.DegArray(np.degrees(target.data))
+        leg_str = '\n'.join([','.join(map(str, row)) for row in target])
+        formatted_data = f"<{leg_str}>"
+        print(f"Gesendete Daten:\n{formatted_data}")
+        self._write(formatted_data)
+
+    def wait4esp32(self, expected_message):
+        while True:
+            try:
+                data = self.sock.recv(1024).decode('utf-8').strip()
+                print(f"Empfangene Daten: {data}")
+                if data == expected_message:
+                    print("Bewegung abgeschlossen!")
+                    break
+            except socket.timeout:
+                time.sleep(0.05)
+
+    def _write(self, message):
+        self.sock.sendall(message.encode('utf-8'))
+        
+    def stop(self):
+        self.running.clear()
+        self.join()
+        self.sock.close()
@@ -0,0 +1,204 @@
+import socket
+import struct
+import time
+import numpy as np
+from threading import Thread, Event
+from queue import Queue
+
+import config as cfg
+import DataTypes as dt
+
+
+# ============================================================
+# Protocol definitions (SHARED with ESP32)
+# ============================================================
+
+PACKET_VERSION = 1
+
+# Packet IDs
+PKT_COMMAND = 1
+PKT_STATUS  = 2
+PKT_DEBUG   = 3
+
+# Field Types
+FT_ARRAY_F32 = 1     # float32 array
+FT_STRING    = 2     # utf-8 string
+FT_UINT8     = 3
+FT_FLOAT32   = 4
+
+# Header: packet_id | version | payload_len | timestamp_ms
+HEADER_FMT  = "<B B H I"
+HEADER_SIZE = struct.calcsize(HEADER_FMT)
+
+# TLV field: type | length | value
+TLV_FMT = "<B H"
+TLV_SIZE = struct.calcsize(TLV_FMT)
+
+
+# ============================================================
+# ESP32 Communication Thread
+# ============================================================
+
+class ESP32Communication(Thread):
+    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
+        super().__init__(daemon=True)
+
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.sock.settimeout(timeout)
+
+        print(f"Connecting to ESP32 at {host}:{port} ...")
+        self.sock.connect((host, port))
+        print("Connected to ESP32!")
+
+        self.tx_queue = Queue()
+        self.rx_queue = Queue()
+        self.running = Event()
+        self.running.set()
+
+    # --------------------------------------------------------
+    # Thread loop
+    # --------------------------------------------------------
+    def run(self):
+        self.sock.setblocking(False)
+
+        while self.running.is_set():
+            self._handle_tx()
+            self._handle_rx()
+            time.sleep(0.002)
+
+    # --------------------------------------------------------
+    # Public API
+    # --------------------------------------------------------
+    def send_command(self, radial_array :dt.RadArray, state=0, name=""):
+        """
+        Example command packet:
+        - 3x6 float array
+        - uint8 state
+        - optional string
+        """
+        data = np.degrees(radial_array).astype(np.uint8)
+        print(data)
+        fields = [
+            self._tlv_array(data),
+            self._tlv_uint8(state)
+        ]
+
+        if name:
+            fields.append(self._tlv_string(name))
+
+        packet = self._build_packet(PKT_COMMAND, fields)
+        self.tx_queue.put(packet)
+
+    def receive(self):
+        """
+        Non-blocking receive.
+        Returns (packet_id, timestamp, fields) or None
+        """
+        try:
+            return self.rx_queue.get_nowait()
+        except Exception:
+            return None
+
+    def stop(self):
+        self.running.clear()
+        self.join()
+        self.sock.close()
+
+    # --------------------------------------------------------
+    # TX / RX handlers
+    # --------------------------------------------------------
+    def _handle_tx(self):
+        if not self.tx_queue.empty():
+            data = self.tx_queue.get()
+            self.sock.sendall(data)
+
+    def _handle_rx(self):
+        try:
+            header = self._recv_exact(HEADER_SIZE)
+            if not header:
+                return
+
+            packet_id, version, payload_len, timestamp = struct.unpack(HEADER_FMT, header)
+
+            if payload_len == 0:
+                # Nothing to parse
+                self.rx_queue.put((packet_id, timestamp, []))
+                return
+
+            payload = self._recv_exact(payload_len)
+            if payload is None:
+                return  # incomplete, skip
+
+            fields = self._parse_tlv(payload)
+            self.rx_queue.put((packet_id, timestamp, fields))
+
+        except BlockingIOError:
+            pass
+
+    # --------------------------------------------------------
+    # Packet construction
+    # --------------------------------------------------------
+    def _build_packet(self, packet_id, fields):
+        payload = b"".join(fields)
+
+        header = struct.pack(
+            HEADER_FMT,
+            packet_id,
+            PACKET_VERSION,
+            len(payload),
+            int(time.time() * 1000) & 0xFFFFFFFF
+        )
+
+        return header + payload
+
+    # --------------------------------------------------------
+    # TLV helpers
+    # --------------------------------------------------------
+    def _tlv(self, ftype, data):
+        return struct.pack(TLV_FMT, ftype, len(data)) + data
+
+    def _tlv_array(self, array):
+        # Convert to uint8 degrees before sending
+        arr_uint8 = array.astype(np.uint8)
+        return self._tlv(FT_ARRAY_F32, arr_uint8.tobytes())
+
+    def _tlv_string(self, text):
+        return self._tlv(FT_STRING, text.encode("utf-8"))
+
+    def _tlv_uint8(self, value):
+        return self._tlv(FT_UINT8, struct.pack("<B", value))
+
+    # --------------------------------------------------------
+    # TLV parsing
+    # --------------------------------------------------------
+    def _parse_tlv(self, payload):
+        pos = 0
+        fields = []
+
+        while pos < len(payload):
+            ftype, flen = struct.unpack(
+                TLV_FMT, payload[pos:pos + TLV_SIZE]
+            )
+            pos += TLV_SIZE
+
+            data = payload[pos:pos + flen]
+            pos += flen
+
+            fields.append((ftype, data))
+
+        return fields
+
+    # --------------------------------------------------------
+    # Socket helper
+    # --------------------------------------------------------
+    def _recv_exact(self, size):
+        data = b""
+        while len(data) < size:
+            try:
+                chunk = self.sock.recv(size - len(data))
+                if not chunk:
+                    return None
+                data += chunk
+            except BlockingIOError:
+                return None
+        return data
@@ -0,0 +1,78 @@
+import numpy as np
+from ArduinoCommunication import ArduinoCommunication
+from EspCommunication import ESP32Communication
+from simulation import Simulation
+import DataTypes as dt
+import kinematics as kin
+import config as cfg
+
+# Globale Variablen
+robotCommunication = None
+shared_sim = None
+if cfg.sim:
+    shared_sim = Simulation()
+else:
+    if cfg.arduinoConnection:
+        robotCommunication = ArduinoCommunication()
+    else:
+        robotCommunication = ESP32Communication()
+
+vector_dirmov = [0, 0]  # Direction Movement
+current_rad: dt.RadArray  # Current Rad
+current_pos: dt.PosArray  # Current Position
+
+# current_deg: dt.DegArray  # Current Degrees
+# legarray: dt.DegArray  # Working Leg Array
+# target_pos: dt.PosArray  # Target Position
+
+robot_state = "idle"  # Current State
+# Init for 6 Legs
+#leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
+# Init for 4 Legs
+leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])
+control_pause: bool = False
+
+# Initilize mit Start Position
+#init_deg: dt.DegArray = dt.DegArray(
+#    [
+#        [90, 30, 115],
+#        [90, 30, 115],
+#        [90, 30, 115],
+#        [90, 150, 65],
+#        [90, 150, 65],
+#        [90, 150, 65],
+#    ]
+#)
+init_deg: dt.DegArray = dt.DegArray(
+    [
+        [90, 30, 95],
+        [90, 30, 95],
+        [90, 30, 95],
+        [90, 150, 85],
+        [90, 150, 85],
+        [90, 150, 85],
+    ]
+)
+#init_deg: dt.DegArray = dt.DegArray(
+#    [
+#        [90, 45, 140],
+#        [90, 45, 140],
+#        [90, 45, 140],
+#        [90, 135, 40],
+#        [90, 135, 40],
+#        [90, 135, 40],
+#    ]
+#)
+
+init90_deg: dt.DegArray = dt.DegArray(
+    [
+        [90, 90, 90],
+        [90, 90, 90],
+        [90, 90, 90],
+        [90, 90, 90],
+        [90, 90, 90],
+        [90, 90, 90],
+    ]
+)
+
+center_points: dt.PosArray = kin.ikpyForward(init_deg.to_rad())
@@ -0,0 +1,5 @@
+for theta1 in np.linspace(min1, max1, 50):
+    for theta2 in np.linspace(min2, max2, 50):
+        for theta3 in np.linspace(min3, max3, 50):
+            xyz = forward_kinematics([theta1, theta2, theta3])
+            # collect (x,y,z)
@@ -0,0 +1,157 @@
+import tkinter as tk
+from tkinter import ttk
+
+# --- Conversion functions ---
+def nm_to_kgcm(tau_nm):
+    return tau_nm * 10.197
+
+# --- Torque calculation function ---
+def calculate():
+    # Get user inputs
+    mass_total = float(mass_total_var.get())
+    legs_supporting = int(legs_supporting_var.get())
+    g = 9.81
+    coxa_len = float(coxa_len_var.get())
+    femur_len = float(femur_len_var.get())
+    tibia_len = float(tibia_len_var.get())
+    safety_factor = float(safety_factor_var.get())
+    servo_torque_max = float(servo_torque_max_var.get())
+
+    # Derived values
+    mass_per_leg = mass_total / legs_supporting
+    force_per_leg = mass_per_leg * g
+
+    # Torques (Nm)
+    tau_tibia = force_per_leg * tibia_len
+    tau_femur = force_per_leg * (femur_len + tibia_len)
+    tau_coxa = force_per_leg * coxa_len
+
+    # Apply safety factor
+    tau_tibia_req = tau_tibia * safety_factor
+    tau_femur_req = tau_femur * safety_factor
+    tau_coxa_req = tau_coxa * safety_factor
+
+    # Convert to kg·cm
+    coxa_kgcm = nm_to_kgcm(tau_coxa)
+    femur_kgcm = nm_to_kgcm(tau_femur)
+    tibia_kgcm = nm_to_kgcm(tau_tibia)
+    coxa_kgcm_req = nm_to_kgcm(tau_coxa_req)
+    femur_kgcm_req = nm_to_kgcm(tau_femur_req)
+    tibia_kgcm_req = nm_to_kgcm(tau_tibia_req)
+
+    # Update info labels
+    info_label.config(
+        text=(
+            f"Mass per leg: {mass_per_leg:.3f} kg\n"
+            f"Force per leg: {force_per_leg:.2f} N\n"
+            f"Servo max torque: {servo_torque_max:.2f} kg·cm\n"
+            f"Safety Factor: x{safety_factor:.2f}"
+        )
+    )
+
+    # Update progress bars
+    update_bar(coxa_bar, coxa_val, coxa_kgcm, servo_torque_max)
+    update_bar(femur_bar, femur_val, femur_kgcm, servo_torque_max)
+    update_bar(tibia_bar, tibia_val, tibia_kgcm, servo_torque_max)
+
+    update_bar(coxa_bar_req, coxa_val_req, coxa_kgcm_req, servo_torque_max)
+    update_bar(femur_bar_req, femur_val_req, femur_kgcm_req, servo_torque_max)
+    update_bar(tibia_bar_req, tibia_val_req, tibia_kgcm_req, servo_torque_max)
+
+
+def update_bar(bar, label, value, limit):
+    ratio = min(value / limit, 1.0)
+    percent = (value / limit) * 100
+    bar["value"] = percent if percent <= 100 else 100
+    label.config(text=f"{value:.2f} / {limit:.2f} kg·cm")
+    if value <= limit:
+        bar.configure(style="Green.Horizontal.TProgressbar")
+    else:
+        bar.configure(style="Red.Horizontal.TProgressbar")
+
+
+# --- Tkinter GUI ---
+root = tk.Tk()
+root.title("Hexapod Torque Estimator (Live)")
+root.geometry("700x700")
+root.resizable(False, False)
+
+mainframe = ttk.Frame(root, padding=10)
+mainframe.pack(fill="both", expand=True)
+
+# Progress bar styles
+style = ttk.Style(root)
+style.theme_use("clam")
+style.configure("Green.Horizontal.TProgressbar", troughcolor="#333", background="#4CAF50")
+style.configure("Red.Horizontal.TProgressbar", troughcolor="#333", background="#F44336")
+
+# Input variables
+mass_total_var = tk.DoubleVar(value=1.08)
+legs_supporting_var = tk.IntVar(value=3)
+coxa_len_var = tk.DoubleVar(value=0.056)
+femur_len_var = tk.DoubleVar(value=0.072)
+tibia_len_var = tk.DoubleVar(value=0.07)
+safety_factor_var = tk.DoubleVar(value=4.5)
+servo_torque_max_var = tk.DoubleVar(value=2.3)
+
+# --- Layout ---
+def add_input(label, variable, from_, to, step=0.01):
+    frame = ttk.Frame(mainframe)
+    frame.pack(fill="x", pady=3)
+    ttk.Label(frame, text=label, width=25).pack(side="left")
+    entry = ttk.Entry(frame, textvariable=variable, width=8)
+    entry.pack(side="left")
+    scale = ttk.Scale(frame, variable=variable, from_=from_, to=to, command=lambda e: calculate())
+    scale.pack(side="left", fill="x", expand=True, padx=10)
+    return entry
+
+add_input("Total Mass [kg]", mass_total_var, 0.1, 5.0)
+
+# Integer spinbox for legs supporting
+frame = ttk.Frame(mainframe)
+frame.pack(fill="x", pady=3)
+ttk.Label(frame, text="Legs Supporting", width=25).pack(side="left")
+spin = ttk.Spinbox(frame, from_=1, to=6, textvariable=legs_supporting_var, width=8, command=calculate)
+spin.pack(side="left")
+
+add_input("Coxa Length [m]", coxa_len_var, 0.03, 0.1)
+add_input("Femur Length [m]", femur_len_var, 0.03, 0.15)
+add_input("Tibia Length [m]", tibia_len_var, 0.03, 0.15)
+add_input("Safety Factor", safety_factor_var, 1.0, 3.0)
+add_input("Servo Torque Max [kg·cm]", servo_torque_max_var, 1.0, 5.0)
+
+# Info label
+info_label = ttk.Label(mainframe, text="", justify="left", font=("Courier", 10))
+info_label.pack(pady=10)
+
+# Function to create a labeled progress bar
+def add_progress(label_text):
+    frame = ttk.Frame(mainframe)
+    frame.pack(fill="x", pady=3)
+    ttk.Label(frame, text=label_text, width=10).pack(side="left")
+    bar = ttk.Progressbar(frame, length=400, maximum=100)
+    bar.pack(side="left", padx=5)
+    val_label = ttk.Label(frame, width=20, anchor="w")
+    val_label.pack(side="left")
+    return bar, val_label
+
+# --- Without Safety Factor ---
+ttk.Label(mainframe, text="Without Safety Factor", font=("Helvetica", 11, "bold")).pack(pady=(15, 3))
+coxa_bar, coxa_val = add_progress("Coxa")
+femur_bar, femur_val = add_progress("Femur")
+tibia_bar, tibia_val = add_progress("Tibia")
+
+# --- With Safety Factor ---
+ttk.Label(mainframe, text="With Safety Factor", font=("Helvetica", 11, "bold")).pack(pady=(15, 3))
+coxa_bar_req, coxa_val_req = add_progress("Coxa")
+femur_bar_req, femur_val_req = add_progress("Femur")
+tibia_bar_req, tibia_val_req = add_progress("Tibia")
+
+# Auto-update on variable changes
+for var in [mass_total_var, legs_supporting_var, coxa_len_var, femur_len_var, tibia_len_var, safety_factor_var, servo_torque_max_var]:
+    var.trace_add("write", lambda *args: calculate())
+
+# Initial calculation
+calculate()
+
+root.mainloop()
@@ -0,0 +1,672 @@
+<robot name="JackBot">
+
+	<!-- Base -->
+	<link name="base_link">
+		<inertial>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <mass value="1"/>
+		  <inertia
+			ixx="0.001" ixy="0" ixz="0"
+			iyy="0.001" iyz="0"
+			izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<box size="0.160 0.120 0.090"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="red">
+				<color rgba="1 0 0 0.5"/>
+			</material>
+		</visual>
+	</link>
+
+	<!-- LEG 1 -->
+	<link name="leg1_coxa">
+		<inertial>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <mass value="1"/>
+		  <inertia
+			ixx="0.001" ixy="0" ixz="0"
+			iyy="0.001" iyz="0"
+			izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg1_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg1_coxa"/>
+		<origin xyz="0.052 0.072 0" rpy="0 0 -0.7854"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg1_femur">
+		<inertial>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <mass value="1"/>
+		  <inertia
+			ixx="0.001" ixy="0" ixz="0"
+			iyy="0.001" iyz="0"
+			izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg1_femur_joint" type="revolute">
+		<parent link="leg1_coxa"/>
+		<child link="leg1_femur"/>
+		<origin xyz="0.068 0 0" rpy="0 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg1_tibia">
+		<inertial>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <mass value="1"/>
+		  <inertia
+			ixx="0.001" ixy="0" ixz="0"
+			iyy="0.001" iyz="0"
+			izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg1_tibia_joint" type="revolute">
+		<parent link="leg1_femur"/>
+		<child link="leg1_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg1_tip">
+		<inertial>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <mass value="1"/>
+		  <inertia
+			ixx="0.001" ixy="0" ixz="0"
+			iyy="0.001" iyz="0"
+			izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg1_tip_joint" type="fixed">
+		<parent link="leg1_tibia"/>
+		<child link="leg1_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+	<!-- LEG 2 -->
+	<link name="leg2_coxa">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg2_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg2_coxa"/>
+		<origin xyz="0 0.070 0" rpy="0 0 0"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg2_femur">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg2_femur_joint" type="revolute">
+		<parent link="leg2_coxa"/>
+		<child link="leg2_femur"/>
+		<origin xyz="0.068 0 0" rpy="0 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg2_tibia">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg2_tibia_joint" type="revolute">
+		<parent link="leg2_femur"/>
+		<child link="leg2_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg2_tip">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg2_tip_joint" type="fixed">
+		<parent link="leg2_tibia"/>
+		<child link="leg2_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+	<!-- LEG 3 -->
+	<link name="leg3_coxa">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg3_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg3_coxa"/>
+		<origin xyz="-0.052 0.072 0" rpy="0 0 0.7854"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg3_femur">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg3_femur_joint" type="revolute">
+		<parent link="leg3_coxa"/>
+		<child link="leg3_femur"/>
+		<origin xyz="0.068 0 0" rpy="0 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg3_tibia">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg3_tibia_joint" type="revolute">
+		<parent link="leg3_femur"/>
+		<child link="leg3_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg3_tip">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg3_tip_joint" type="fixed">
+		<parent link="leg3_tibia"/>
+		<child link="leg3_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+	<!-- LEG 4 -->
+	<link name="leg4_coxa">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg4_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg4_coxa"/>
+		<origin xyz="-0.052 -0.072 0" rpy="0 0 2.3562"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg4_femur">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg4_femur_joint" type="revolute">
+		<parent link="leg4_coxa"/>
+		<child link="leg4_femur"/>
+		<origin xyz="0.068 0 0" rpy="3.1416 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg4_tibia">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg4_tibia_joint" type="revolute">
+		<parent link="leg4_femur"/>
+		<child link="leg4_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg4_tip">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg4_tip_joint" type="fixed">
+		<parent link="leg4_tibia"/>
+		<child link="leg4_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+	<!-- LEG 5 -->
+	<link name="leg5_coxa">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg5_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg5_coxa"/>
+		<origin xyz="0 -0.070 0" rpy="0 0 3.1416"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg5_femur">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg5_femur_joint" type="revolute">
+		<parent link="leg5_coxa"/>
+		<child link="leg5_femur"/>
+		<origin xyz="0.068 0 0" rpy="3.1416 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg5_tibia">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg5_tibia_joint" type="revolute">
+		<parent link="leg5_femur"/>
+		<child link="leg5_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg5_tip">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg5_tip_joint" type="fixed">
+		<parent link="leg5_tibia"/>
+		<child link="leg5_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+	<!-- LEG 6 -->
+	<link name="leg6_coxa">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.068" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.028 0 0" rpy="0 1.5708 0"/>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg6_coxa_joint" type="revolute">
+		<parent link="base_link"/>
+		<child link="leg6_coxa"/>
+		<origin xyz="0.052 -0.072 0" rpy="0 0 -2.3562"/>
+		<!-- position from base -->
+		<axis xyz="0 0 1"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg6_femur">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.0602" radius="0.005"/>
+			</geometry>
+			<origin xyz="0 0 -0.0365" rpy="0 0 0"/>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg6_femur_joint" type="revolute">
+		<parent link="leg6_coxa"/>
+		<child link="leg6_femur"/>
+		<origin xyz="0.068 0 0" rpy="3.1416 0 0"/>
+		<axis xyz="0 -1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg6_tibia">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<cylinder length="0.07055" radius="0.005"/>
+			</geometry>
+			<origin xyz="0.035 0 0" rpy="0 1.5708 0"/>
+			<material name="yellow">
+				<color rgba="0 1 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg6_tibia_joint" type="revolute">
+		<parent link="leg6_femur"/>
+		<child link="leg6_tibia"/>
+		<origin xyz="0 0 -0.0602" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<limit lower="0.2618" upper="2.8798" effort="1" velocity="1"/>
+	</joint>
+
+	<link name="leg6_tip">
+		<inertial>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<mass value="1"/>
+			<inertia
+			  ixx="0.001" ixy="0" ixz="0"
+			  iyy="0.001" iyz="0"
+			  izz="0.001"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<sphere radius="0.01"/>
+			</geometry>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<material name="purple">
+				<color rgba="1 0 1 1"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="leg6_tip_joint" type="fixed">
+		<parent link="leg6_tibia"/>
+		<child link="leg6_tip"/>
+		<origin xyz="0.07055 0 0" rpy="0 0 0"/>
+	</joint>
+
+</robot>
@@ -0,0 +1,50 @@
+
+# JackBot — Hexapod Control & Simulation
+
+**Purpose**
+- **Overview**: JackBot is a Python project for controlling and simulating a six-legged (hexapod) robot. It computes foot trajectories, runs inverse kinematics against a URDF model, and sends joint commands to either a hardware controller (ESP32 / Arduino) or a PyBullet simulation.
+
+**Quick Start**
+- **Simulation**: enable the simulator in `config.py` by setting `sim = True`, then run:
+
+```bash
+python3 main.py
+```
+- **Hardware**: set `sim = False` in `config.py` and choose `arduinoConnection = True` or provide your ESP32 settings. Verify `port`, `baudrate`, `esp32_ip`, and `esp32_port` in `config.py`.
+
+**Dependencies**
+- **Python packages**: At minimum the project uses `numpy`, `pygame`, `ikpy`, `pybullet`, `pyserial`, and `matplotlib`.
+- **Install** (recommended inside a venv):
+
+```bash
+python3 -m venv .venv
+source .venv/bin/activate
+pip install numpy pygame ikpy pybullet pyserial matplotlib
+```
+
+**Project structure (key files)**
+- **Entry point**: [main.py](main.py) — starts the controller and state loop.
+- **Controller**: [Controller.py](Controller.py) — joystick input and `ControlIntent` updates.
+- **Kinematics**: [kinematics.py](kinematics.py) — URDF-based forward/inverse kinematics using IKPy.
+- **Simulation**: [simulation.py](simulation.py) — PyBullet visualization and joint control.
+- **Hardware comms**: [EspCommunication.py](EspCommunication.py) and [ArduinoCommunication.py](ArduinoCommunication.py) — send commands to ESP32 or Arduino.
+- **Runtime globals**: [GlobalVariables.py](GlobalVariables.py) — central runtime objects and flags.
+- **Types**: [DataTypes.py](DataTypes.py) — `PosArray`, `DegArray`, `RadArray`, `ControlIntent`.
+- **States**: [RobotState/idle.py](RobotState/idle.py) and [RobotState/walking.py](RobotState/walking.py) — state machine implementations.
+- **Config**: [config.py](config.py) — ports, flags (`sim`, `arduinoConnection`), URDF path, timing values.
+
+**How it works**
+- `main.py` creates a `ControlIntent` and `RobotContext`, starts the controller thread, and runs a state loop. The `WalkingState` computes per-leg foot paths (Bezier/linear), converts positions to joint angles with `kinematics.ikpyInverse()`, then sends commands to hardware or updates the simulator.
+
+**Configuration notes**
+- **URDF**: the robot description file is `JackBotUrdf.urdf`; ensure the path in `config.py` (`urdf_path`) is correct.
+- **Timing**: step timing and tick rate are configured in `config.py` (`standard_duration`, `standard_tickpersec`).
+
+**Suggested next steps**
+- Add a `requirements.txt` or `pyproject.toml` for reproducible installs.
+- Document hardware wiring and expected serial/ESP packet formats if you plan to use hardware.
+- Provide an example `config.py` for common setups (sim vs hardware).
+
+**License & Contributing**
+- No license file included. Add `LICENSE` if you want to publish or share.
+- Contributions: open an issue or PR with improvements; add tests for kinematics if possible.
@@ -0,0 +1,20 @@
+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
@@ -0,0 +1,7 @@
+import RobotState
+
+class IdleState(RobotState):
+    def update(self, ctx, intent, dt):
+        if intent.walk:
+            return "walking"
+        return None
@@ -0,0 +1,70 @@
+# RobotState/walking.py
+import time
+import numpy as np
+import config as cfg
+import kinematics as kin
+import DataTypes as dt
+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,29 @@
+# Connection
+port = "COM4"
+baudrate = 38400
+
+esp32_ip = "192.168.188.32"
+esp32_port = 3323
+
+timeout = 2.0
+
+sim: bool = False
+arduinoConnection: bool = False
+urdf_path = "JackBotUrdf.urdf"
+
+# row = legnumber (left 0,1,2 right 3,4,5)
+# column = servo position from torso(0) to feet(2)
+
+# Dimensions in m
+#robot_height = -0.1
+step_height = 0.05
+step_length = 0.08
+# Leglength in m
+# L1 = 0.068
+# L2 = 0.0602
+# L3 = 0.070
+
+# standard_tickpersec = # 20 Fluessige Bewegung
+standard_tickpersec:float = 25 # [ticks/s]
+standard_duration:float = 0.4  # [s],ip
+standard_tickduration:float = 1 / standard_tickpersec
@@ -0,0 +1,293 @@
+from ikpy.chain import Chain
+from ikpy.link import OriginLink, URDFLink
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+import time
+
+import GlobalVariables as gv
+import config as cfg
+import DataTypes as dt
+
+leg_chains = {
+    0: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg1_coxa_joint",
+            "leg1_coxa",
+            "leg1_femur_joint",
+            "leg1_femur",
+            "leg1_tibia_joint",
+            "leg1_tibia",
+            "leg1_tip_joint",
+            "leg1_tip",
+        ],
+    ),
+    1: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg2_coxa_joint",
+            "leg2_coxa",
+            "leg2_femur_joint",
+            "leg2_femur",
+            "leg2_tibia_joint",
+            "leg2_tibia",
+            "leg2_tip_joint",
+            "leg2_tip",
+        ],
+    ),
+    2: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg3_coxa_joint",
+            "leg3_coxa",
+            "leg3_femur_joint",
+            "leg3_femur",
+            "leg3_tibia_joint",
+            "leg3_tibia",
+            "leg3_tip_joint",
+            "leg3_tip",
+        ],
+    ),
+    3: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg4_coxa_joint",
+            "leg4_coxa",
+            "leg4_femur_joint",
+            "leg4_femur",
+            "leg4_tibia_joint",
+            "leg4_tibia",
+            "leg4_tip_joint",
+            "leg4_tip",
+        ],
+    ),
+    4: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg5_coxa_joint",
+            "leg5_coxa",
+            "leg5_femur_joint",
+            "leg5_femur",
+            "leg5_tibia_joint",
+            "leg5_tibia",
+            "leg5_tip_joint",
+            "leg5_tip",
+        ],
+    ),
+    5: Chain.from_urdf_file(
+        cfg.urdf_path,
+        base_elements=[
+            "base_link",
+            "leg6_coxa_joint",
+            "leg6_coxa",
+            "leg6_femur_joint",
+            "leg6_femur",
+            "leg6_tibia_joint",
+            "leg6_tibia",
+            "leg6_tip_joint",
+            "leg6_tip",
+        ],
+    ),
+}
+
+for leg in leg_chains.values():
+    leg.active_links_mask = [False, True, True, True, False]
+
+
+def ikpyForward(target_rad: dt.RadArray) -> dt.PosArray:
+    target_pos_temp = []
+
+    for leg_id, chain in leg_chains.items():
+        # Forward kinematics -> 4x4 matrix
+        fk_matrix = chain.forward_kinematics([0] + list(target_rad[leg_id]) + [0])
+        # Extract translation vector (x, y, z)
+        x, y, z = fk_matrix[:3, 3]
+        target_pos_temp.append([x, y, z])
+    return dt.RadArray(np.array(target_pos_temp))
+
+
+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)
+
+        try:
+            ik_result = chain.inverse_kinematics(
+                target_pos[leg_id],
+                initial_position=guess,
+                max_iter=100
+            )
+        except ValueError:
+            # fallback → keep the clipped guess
+            ik_result = guess
+
+        # keep only the 3 actuated joint angles (indices 1,2,3)
+        target_rad_temp.append(ik_result[1:4])
+
+    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))
+
+
+def ikpytest():
+    targets: dt.PosArray = dt.PosArray(
+        [
+            [0.047, 0.272, 0],
+            [0, 0.272, 0],
+            [-0.047, 0.272, 0],
+            [-0.047, -0.272, 0],
+            [0, -0.272, 0],
+            [0.047, -0.272, 0],
+        ]
+    )
+    joint_angles_deg: dt.DegArray = dt.DegArray(
+        [
+            [90, 90, 90],
+            [90, 90, 90],
+            [90, 90, 90],
+            [90, 90, 90],
+            [90, 90, 90],
+            [90, 90, 90],
+        ]
+    )
+    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()))
@@ -0,0 +1,61 @@
+# Global Variables
+import GlobalVariables as gv
+
+# main.py
+import time
+import threading
+
+from DataTypes import ControlIntent
+from RobotState.RobotState import RobotContext
+from Controller import controller_loop
+from RobotState.idle import IdleState
+from RobotState.walking import WalkingState
+
+import DataTypes as dt
+
+def main():
+    intent = ControlIntent()
+    ctx = RobotContext()
+
+    ctx.center_points = dt.PosArray([
+        [50, 50, -80],
+        [50,-50, -80],
+        [0, 70, -80],
+        [0,-70, -80],
+        [-50,50, -80],
+        [-50,-50,-80]
+    ])
+    ctx.current_pos = ctx.center_points.copy()
+
+    controller_thread = threading.Thread(
+        target=controller_loop,
+        args=(intent,),
+        daemon=True
+    )
+    controller_thread.start()
+
+    states = {
+        "idle": IdleState(),
+        "walking": WalkingState()
+    }
+
+    current = states["idle"]
+    current.on_enter(ctx)
+
+    last = time.perf_counter()
+
+    while not intent.quit:
+        now = time.perf_counter()
+        dt_s = now - last
+        last = now
+
+        next_state = current.update(ctx, intent, dt_s)
+        if next_state:
+            current.on_exit(ctx)
+            current = states[next_state]
+            current.on_enter(ctx)
+
+        time.sleep(0.01)
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,68 @@
+import pybullet as p
+import numpy as np
+import RobotState as rs
+import GlobalVariables as gv
+import config as cfg
+import DataTypes as dt
+import time
+import os
+import math
+
+
+class Simulation:
+    def __init__(self):
+        self.physics_client = p.connect(p.GUI)
+        self.robot = p.loadURDF(cfg.urdf_path, useFixedBase=True)
+
+        self.revolute_joints = [
+            i
+            for i in range(p.getNumJoints(self.robot))
+            if p.getJointInfo(self.robot, i)[2] == p.JOINT_REVOLUTE
+        ]
+
+        self.set_all_joints_to_90()
+        p.resetDebugVisualizerCamera(
+            cameraDistance=1.0,
+            cameraYaw=50,
+            cameraPitch=-35,
+            cameraTargetPosition=[0, 0, 0],
+        )
+
+    def set_all_joints_to_90(self):
+        for joint_index in range(p.getNumJoints(self.robot)):
+            joint_info = p.getJointInfo(self.robot, joint_index)
+            joint_type = joint_info[2]
+            if joint_type == p.JOINT_REVOLUTE:
+                p.resetJointState(self.robot, joint_index, math.radians(90))
+
+    def updatePos(self, current_rad: dt.RadArray):
+        radflat = current_rad.data.flatten()
+        for joint_index, target_angle in zip(self.revolute_joints, radflat):
+            p.setJointMotorControl2(
+                bodyIndex=self.robot,
+                jointIndex=joint_index,
+                controlMode=p.POSITION_CONTROL,
+                targetPosition=target_angle,
+                force=500,
+            )
+
+    def step(self):
+        p.stepSimulation()
+
+    def disconnect(self):
+        p.disconnect(self.physics_client)
+
+
+if __name__ == "__main__":
+    count = 0
+    current_rad: dt.RadArray = gv.init_deg.to_rad()
+    gv.shared_sim.updatePos(current_rad)
+
+    rs.walking()
+    while True:
+        count = +1
+        gv.shared_sim.step()
+        time.sleep(1 / 240)
+        if count > 50:
+            rs.walking()
+            count = 0