293 lines
8.8 KiB
Python
293 lines
8.8 KiB
Python
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())) |