JackM323
2b2125bfde
old code was uploaded in the initial upload changed everything with working programm and updated the readme
319 lines
11 KiB
Python
319 lines
11 KiB
Python
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 |