JackM323
2b2125bfde
old code was uploaded in the initial upload changed everything with working programm and updated the readme
292 lines
8.8 KiB
Python
292 lines
8.8 KiB
Python
from ikpy.chain import Chain
|
|
from ikpy.link import OriginLink, URDFLink
|
|
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())) |