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)方法 |