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()))