import lcm import sys import os import time from threading import Thread, Lock from robot_control_cmd_lcmt import robot_control_cmd_lcmt from robot_control_response_lcmt import robot_control_response_lcmt from localization_lcmt import localization_lcmt import toml import copy import math from file_send_lcmt import file_send_lcmt class Vec3: def __init__(self, x=0, y=0, z=0): self.x = x self.y = y self.z = z def __getitem__(self, key): if key == 0: return self.x elif key == 1: return self.y elif key == 2: return self.z else: raise IndexError("Vec3 index out of range") def __setitem__(self, key, value): if key == 0: self.x = value elif key == 1: self.y = value elif key == 2: self.z = value else: raise IndexError("Vec3 index out of range") class Motor_Ctrl(object): def InverseKinematic(quad, leg, p): # 假设quad是一个Quadruped实例,包含必要的属性 # p是一个Vec3实例或任何可以通过索引访问x, y, z的类似对象 l1 = quad.abad_link_length_ l2 = quad.hip_link_length_ l3 = quad.knee_link_length_ l4 = quad.knee_link_y_offset_ x, y, z = p.x, p.y, p.z if abs(y) < 0.0001: y = 0.0001 ss1 = math.sqrt(z**2 + y**2) side_sign = quad.GetSideSign(leg) # 假设Quadruped类有一个GetSideSign方法 if side_sign * y >= 0: q_des_0 = side_sign * (math.acos((z**2 + ss1**2 - y**2) / (2 * abs(z) * ss1)) + math.acos((l1 + l4) / ss1) - math.pi / 2) else: q_des_0 = side_sign * (-math.acos((z**2 + ss1**2 - y**2) / (2 * abs(z) * ss1)) + math.acos((l1 + l4) / ss1) - math.pi / 2) ss2 = math.sqrt(ss1**2 - (l1 + l4)**2 + x**2) q_des_2 = math.acos((l2**2 + l3**2 - ss2**2) / (2 * l2 * l3)) q_des_2 = math.pi - q_des_2 if z <= 0: q_des_1 = -math.acos((l2**2 + ss2**2 - l3**2) / (2 * l2 * ss2)) + math.asin(x / ss2) else: q_des_1 = -math.acos((l2**2 + ss2**2 - l3**2) / (2 * l2 * ss2)) + (math.pi - math.asin(x / ss2)) # 检查NaN值(虽然Python的math函数通常不会直接返回NaN,但这里为了完整性还是检查) if math.isnan(q_des_0) or math.isnan(q_des_1) or math.isnan(q_des_2): # print(f"[LegController] leg {leg} IK result contains NaN!") return None # 或者可以选择抛出异常 return Vec3(q_des_0, q_des_1, q_des_2) # 注意:这里需要Quadruped类的定义,以及它包含必要的属性和方法,如abad_link_length_, hip_link_length_等 # 以及GetSideSign(leg)方法