85 lines
3.0 KiB
Python
85 lines
3.0 KiB
Python
|
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)方法
|