Complete Restructered Robot Code
Robot into its own Class instead of lose Global Variables that cause circular imports StateClass usage instead of the old RobotState.py New Input Class for Controller and randome intputs
This commit is contained in:
+32
-152
@@ -1,10 +1,13 @@
|
||||
from ikpy.chain import Chain
|
||||
from ikpy.link import OriginLink, URDFLink
|
||||
from typing import Optional
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
|
||||
import GlobalVariables as gv
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=UserWarning, module="ikpy")
|
||||
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
|
||||
@@ -110,33 +113,38 @@ def ikpyForward(target_rad: dt.RadArray) -> dt.PosArray:
|
||||
target_pos_temp.append([x, y, z])
|
||||
return dt.RadArray(np.array(target_pos_temp))
|
||||
|
||||
def ikpyInverse(target_pos: dt.PosArray, initial_rad: Optional[dt.RadArray] = None) -> dt.RadArray:
|
||||
calculated_rads = []
|
||||
|
||||
def ikpyInverse(target_pos: dt.PosArray) -> dt.RadArray:
|
||||
target_rad_temp = []
|
||||
print("target_pos:\n",target_pos)
|
||||
for leg_id, chain in leg_chains.items():
|
||||
# prepare initial guess
|
||||
guess = np.array([0] + list(gv.current_rad[leg_id]) + [0], dtype=float)
|
||||
for leg_id in range(6):
|
||||
chain = leg_chains[leg_id] # Your IKPy Chain object
|
||||
target_xyz = target_pos[leg_id]
|
||||
|
||||
try:
|
||||
ik_result = chain.inverse_kinematics(
|
||||
target_pos[leg_id],
|
||||
initial_position=guess,
|
||||
max_iter=100
|
||||
full_initial_position = None
|
||||
if initial_rad is not None:
|
||||
# Create a zero array matching the total number of links in the chain
|
||||
full_initial_position = [0.0] * len(chain.links)
|
||||
|
||||
# Map the 3 active joint angles into active link indices
|
||||
active_indices = [i for i, active in enumerate(chain.active_links_mask) if active]
|
||||
|
||||
# Match 3 active joints to the 3 active link positions in the chain
|
||||
for idx, angle in zip(active_indices, initial_rad[leg_id]):
|
||||
full_initial_position[idx] = angle
|
||||
|
||||
if full_initial_position is not None:
|
||||
angles = chain.inverse_kinematics(
|
||||
target_position=target_xyz,
|
||||
initial_position=full_initial_position
|
||||
)
|
||||
except ValueError:
|
||||
# fallback → keep the clipped guess
|
||||
ik_result = guess
|
||||
else:
|
||||
angles = chain.inverse_kinematics(target_position=target_xyz)
|
||||
|
||||
# keep only the 3 actuated joint angles (indices 1,2,3)
|
||||
target_rad_temp.append(ik_result[1:4])
|
||||
# Extract only the active joint angles (3 revolute joints) from IKPy result
|
||||
active_angles = chain.active_from_full(angles)
|
||||
calculated_rads.append(active_angles)
|
||||
|
||||
return dt.RadArray(np.array(target_rad_temp))
|
||||
# for leg_id, chain in leg_chains.items():
|
||||
# ik_result = chain.inverse_kinematics(target_pos[leg_id])
|
||||
# # Remove first element (IKPy adds a "dummy" fixed base joint)
|
||||
# target_rad_temp.append(ik_result[1:4])
|
||||
return dt.RadArray(np.array(target_rad_temp))
|
||||
return dt.RadArray(calculated_rads)
|
||||
|
||||
|
||||
def ikpytest():
|
||||
@@ -161,132 +169,4 @@ def ikpytest():
|
||||
]
|
||||
)
|
||||
current_rad: dt.RadArray = joint_angles_deg.to_rad()
|
||||
ikpyInverse(targets, current_rad)
|
||||
|
||||
|
||||
# walk old
|
||||
"""
|
||||
def walk(duration=standard_duration, ticks=standard_tickrate, curve_height=gv.step_height):
|
||||
tick_duration = duration / ticks
|
||||
tick_positions = np.copy(gv.current_pos)
|
||||
dirmov_copy = gv.vector_dirmov
|
||||
target_pos =[]
|
||||
for i in range(6): # Zielposition berechnen
|
||||
target_pos.append([gv.center_points[i][0] + dirmov_copy[0] * gv.step_length, gv.center_points[i][1] + dirmov_copy[1] * gv.step_length, gv.robot_height ])
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t)**2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
for tick in range(int(ticks) + 1):
|
||||
loop_start = time.perf_counter()
|
||||
t = tick / ticks
|
||||
|
||||
for leg_id in range(6):
|
||||
if gv.leg_state[leg_id] == "drag":
|
||||
# Linear interpolation
|
||||
tick_positions[leg_id] = gv.current_pos[leg_id] + (gv.center_points[leg_id] - gv.current_pos[leg_id]) * t
|
||||
|
||||
elif gv.leg_state[leg_id] == "step":
|
||||
# Curve movement (Bezier path)
|
||||
help_pos = [
|
||||
target_pos[leg_id][0] - gv.current_pos[leg_id][0],
|
||||
target_pos[leg_id][1] - gv.current_pos[leg_id][1],
|
||||
target_pos[leg_id][2] + curve_height
|
||||
]
|
||||
x = interpolate(t, gv.current_pos[leg_id][0], help_pos[0], target_pos[leg_id][0])
|
||||
y = interpolate(t, gv.current_pos[leg_id][1], help_pos[1], target_pos[leg_id][1])
|
||||
z = interpolate(t, gv.current_pos[leg_id][2], help_pos[2], target_pos[leg_id][2])
|
||||
tick_positions[leg_id] = [x, y, z]
|
||||
|
||||
# Send all legs at once through IK
|
||||
ikpyInverse(tick_positions, gv.current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - loop_start
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Final position correction
|
||||
ikpyInverse(target_pos, gv.current_pos)
|
||||
|
||||
if (gv.leg_state[0] == "step"):
|
||||
gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
|
||||
if (gv.leg_state[0] == "drag"):
|
||||
gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
|
||||
# Berechnung der Bein Bewegung - Grade
|
||||
def drag_leg(
|
||||
leg_num,
|
||||
current_pos,
|
||||
target_pos,
|
||||
duration=cfg.standard_duration,
|
||||
ticks=cfg.standard_tickrate,
|
||||
):
|
||||
tick_duration = duration / ticks
|
||||
dragtickvec = (target_pos - current_pos[leg_num]) / ticks
|
||||
|
||||
tick_positions = np.copy(current_pos)
|
||||
|
||||
for tick in range(int(ticks)):
|
||||
start_time = time.perf_counter()
|
||||
|
||||
tick_positions[leg_num] += dragtickvec
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - start_time
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
tick_positions[leg_num] = target_pos
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
# Berechnung der Bein Bewegung - Kurve
|
||||
def curve_leg(
|
||||
leg_num,
|
||||
current_pos,
|
||||
target_pos,
|
||||
curve_height=cfg.step_height,
|
||||
duration=cfg.standard_duration,
|
||||
ticks=cfg.standard_tickrate,
|
||||
):
|
||||
tick_duration = duration / ticks
|
||||
|
||||
help_pos = [
|
||||
target_pos[0] - current_pos[leg_num][0],
|
||||
target_pos[1] - current_pos[leg_num][1],
|
||||
target_pos[2] + curve_height,
|
||||
]
|
||||
|
||||
def interpolate(t, p0, p1, p2):
|
||||
return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2
|
||||
|
||||
tick_positions = np.copy(current_pos)
|
||||
|
||||
for tick in range(int(ticks) + 1):
|
||||
start_time = time.perf_counter()
|
||||
|
||||
t = tick / ticks
|
||||
x = interpolate(t, current_pos[leg_num][0], help_pos[0], target_pos[0])
|
||||
y = interpolate(t, current_pos[leg_num][1], help_pos[1], target_pos[1])
|
||||
z = interpolate(t, current_pos[leg_num][2], help_pos[2], target_pos[2])
|
||||
|
||||
tick_positions[leg_num] = [x, y, z]
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
|
||||
elapsed = time.perf_counter() - start_time
|
||||
sleep_time = tick_duration - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
tick_positions[leg_num] = target_pos
|
||||
ikpyInverse(tick_positions, current_pos)
|
||||
time.sleep(0.05)
|
||||
|
||||
"""
|
||||
if __name__ == "__main__":
|
||||
print(ikpyForward(gv.test_deg.to_rad()))
|
||||
ikpyInverse(targets, current_rad)
|
||||
Reference in New Issue
Block a user