merge conflict and readme update
old code was uploaded in the initial upload changed everything with working programm and updated the readme
This commit is contained in:
+319
@@ -0,0 +1,319 @@
|
||||
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
|
||||
Reference in New Issue
Block a user