meiqi/tienkung-szu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First Commit

Browse Source
This commit is contained in:
meiqi
2026-03-27 16:10:51 +08:00
commit c45245038f
103 changed files with 10994 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

814
Deploy_Tienkung/common/robot_interface.py Normal file
View File

@@ -0,0 +1,814 @@
"""
Robot Interface
Python equivalent of the C++ RobotInterface class
"""
from __future__ import annotations
import queue
from common.peekqueue import PeekableQueue
import yaml
import os
from abc import ABC, abstractmethod
from typing import Dict, Any
import numpy as np
# ROS messages
from bodyctrl_msgs.msg import MotorStatusMsg, CmdMotorCtrl, MotorCtrl, Imu, MotorStatus
import rclpy
from std_msgs.msg import String
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import transforms3d as t3d
from .body_id_map import BodyServoIdMap
from .robot_data import RobotData
import functools
import time
import math
from sptlib_python import funcSPTrans as FuncSPTrans
from common.joystick import ControlFlag
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Float64
from FSM.fsm_base import FSMStateName
def timing_decorator(func):
"""
装饰器：记录函数执行时间
"""
@functools.wraps(func)
def wrapper(*args, **kwargs):
start_time = time.perf_counter()
result = func(*args, **kwargs)
end_time = time.perf_counter()
execution_time = end_time - start_time
# print(f"[TIMING] {func.__name__} executed in {execution_time:.6f} seconds")
return result
return wrapper
class RobotInterface(ABC):
"""机器人接口抽象基类"""
def __init__(self, robot_data: RobotData):
self.robot_data_ = robot_data
@abstractmethod
def init(self, node: Node):
"""初始化接口"""
pass
@abstractmethod
def update_robot_data(self, flag:ControlFlag, time_passed: float):
"""更新机器人状态"""
pass
@abstractmethod
def send_motor_commands(self, flag: ControlFlag):
"""发布电机控制命令"""
pass
class RobotInterfaceImpl(RobotInterface):
"""机器人接口具体实现"""
def __init__(self, robot_data: RobotData, config_path: str = ''):
super().__init__(robot_data)
self.initialized = False
self.node = None
self.config_path = config_path
# ID映射
self.id_map = BodyServoIdMap()
self.id_map.body_can_id_map_init()
# 消息队列
self.queue_leg_motor_state = PeekableQueue(maxsize=1)
self.queue_arm_motor_state = PeekableQueue(maxsize=1)
self.queue_waist_motor_state = PeekableQueue(maxsize=1)
self.queue_imu_xsens = PeekableQueue(maxsize=1)
self.queue_walk_cmd = PeekableQueue(maxsize=1)
# 关节维度
self.floating_base_dof = 6
self.whole_joint_nums = self.id_map.whole_motor_nums + self.floating_base_dof
# 临时变量用于优化计算
self.temp_q = np.empty(self.id_map.whole_motor_nums)
# 预分配另一个用于存储中间计算的临时数组
self._temp_zero_cnt = np.empty(self.id_map.whole_motor_nums)
# 电机控制参数
self.motor_dir = np.ones(self.id_map.whole_motor_nums)
self.zero_cnt = np.zeros(self.id_map.whole_motor_nums)
self.zero_offset = np.zeros(self.id_map.whole_motor_nums)
# 添加标志位，用于跟踪是否是首次接收数据
self.first_leg_data_received = False
self.first_arm_data_received = False
self.first_waist_data_received = False
# 关节限位
self.joint_limits = {}
self.joint_pos_threshold = math.pi
# 串并联转换器
self.fun_s2p = FuncSPTrans()
# 串并联转换相关变量
self.left_ankle_indices = np.array([4, 5]) + self.floating_base_dof
self.right_ankle_indices = np.array([10, 11]) + self.floating_base_dof
self.q_a_p = np.zeros(4) # 并联关节位置
self.qdot_a_p = np.zeros(4) # 并联关节速度
self.tor_a_p = np.zeros(4) # 并联关节力矩
self.ankle_kp_p = np.zeros(4) # 并联关节刚度
self.ankle_kd_p = np.zeros(4) # 并联关节阻尼
# TF相关属性
self.tf_buffer = None
self.tf_listener = None
# ROS publishers and subscribers
self.pub_leg_motor_cmd = None
self.pub_arm_motor_cmd = None
self.pub_waist_motor_cmd = None
self.pub_fsm_state = None
self.sub_leg_state = None
self.sub_arm_state = None
self.sub_waist_state = None
# 当前机器人所处状态
self.current_state: FSMStateName = FSMStateName.STOP
def update_param(self, current_state: FSMStateName = None):
"""更新机器人接口"""
if current_state is not None:
self.current_state = current_state
def load_config(self):
"""从配置文件加载关键参数"""
config_path = self.config_path
if not os.path.exists(config_path):
self.node.get_logger().error(
f"Joint limits config file not found: {config_path}")
raise FileNotFoundError(
f"Joint limits config file not found: {config_path}")
try:
with open(config_path, 'r') as f:
config = yaml.safe_load(f)
except Exception as e:
self.node.get_logger().error(
f"Failed to load joint limits config from {config_path}: {e}")
raise RuntimeError(
f"Failed to load joint limits config from {config_path}: {e}")
# 读取运行模式
self.sim = config.get('sim')
self.debug = config.get('debug')
# 机器人接口配置
interface_config = config.get('robot_interface')
# 是否限位
self.clip_actions = interface_config.get('clip_actions')
# 加载关节限位值
self._load_joint_limits(interface_config)
# 加载控制状态
self._load_control_status(interface_config)
# 零位
self.zero_pos = np.array(interface_config.get('zero_pos'))
# 电流转换比例
self.ct_scale = np.array(interface_config.get('ct_scale'))
# IMU 数据偏移
self.xsense_roll_offset = interface_config.get(
'xsense_data_roll_offset')
# 禁用电机
self.disable_joints_ = interface_config.get('disable_joints', False)
# 脚踝Kp,Kd
self.ankle_kp_p = np.array(interface_config.get('ankle_kp_p'))
self.ankle_kd_p = np.array(interface_config.get('ankle_kd_p'))
def _load_control_status(self, config: Dict[str, Any]):
# 字符串命令到枚举值的映射
state_to_FSMState = {
"STOP": FSMStateName.STOP,
"ZERO": FSMStateName.ZERO,
"WALKAMP": FSMStateName.WALKAMP,
"MYPOLICY": FSMStateName.MYPOLICY,
"XSIMRUN": FSMStateName.XSIMRUN,
}
self.waist_control_status = [state_to_FSMState[state] for state in config.get('waist_control_status')]
self.legs_control_status = [state_to_FSMState[state] for state in config.get('legs_control_status')]
self.arms_control_status = [state_to_FSMState[state] for state in config.get('arms_control_status')]
self.left_arm_only_status = [state_to_FSMState[state] for state in config.get('left_arm_only_status')]
self.right_arm_only_status = [state_to_FSMState[state] for state in config.get('right_arm_only_status')]
def _load_joint_limits(self, config: Dict[str, Any]):
"""从配置文件加载关节限位值"""
# 从配置中获取关节限位信息
joint_limits_config = config.get('joint_limits', None)
if joint_limits_config is None:
error_msg = "No joint_limits section in config"
self.node.get_logger().error(error_msg)
raise ValueError(error_msg)
else:
# 从配置中加载限位值
for joint_name, limits in joint_limits_config.items():
if 'min' in limits and 'max' in limits:
self.joint_limits[joint_name] = {
'min': float(limits['min']),
'max': float(limits['max'])
}
else:
error_msg = f"Invalid limits for joint {joint_name}"
self.node.get_logger().error(error_msg)
raise ValueError(error_msg)
self.node.get_logger().info(f"Loaded joint limits from {config}")
# 预计算ID到限位的映射
self.id_to_limits = {}
for joint_name, limits in self.joint_limits.items():
index = self.id_map.get_index_by_name(joint_name)
if index >= 0:
motor_id = self.id_map.get_id_by_index(index)
self.id_to_limits[motor_id] = limits
# 记录加载的限位值
for joint_name, limits in self.joint_limits.items():
self.node.get_logger().debug(
f"Joint {joint_name}: [{limits['min']}, {limits['max']}]")
print("-" * 30 + '关节限位值' + '-' * 30)
print(self.joint_limits)
def init(self, node: Node):
"""初始化接口"""
self.node = node
self.initialized = True
# 初始化ROS接口
self._init_ros_interfaces()
# 加载配置文件
self.load_config()
node.get_logger().info("Robot interface initialized")
def _init_ros_interfaces(self):
"""初始化ROS接口"""
qos_profile = QoSProfile(
# reliability=ReliabilityPolicy.RELIABLE,
# history=HistoryPolicy.KEEP_LAST,
depth=10)
# 发布者
self.pub_leg_motor_cmd = self.node.create_publisher(
CmdMotorCtrl, '/leg/cmd_ctrl', qos_profile)
self.pub_arm_motor_cmd = self.node.create_publisher(
CmdMotorCtrl, '/arm/cmd_ctrl', qos_profile)
self.pub_waist_motor_cmd = self.node.create_publisher(
CmdMotorCtrl, '/waist/cmd_ctrl', qos_profile)
self.pub_fsm_state = self.node.create_publisher(
String, '/control/fsm_state', qos_profile)
# 订阅者
self.sub_leg_state = self.node.create_subscription(
MotorStatusMsg, '/leg/status', self._leg_motor_status_callback,
qos_profile)
self.sub_arm_state = self.node.create_subscription(
MotorStatusMsg, '/arm/status', self._arm_motor_status_callback,
qos_profile)
self.sub_waist_state = self.node.create_subscription(
MotorStatusMsg, '/waist/status', self._waist_motor_status_callback,
qos_profile)
#（非电机相关）
self.sub_imu_xsens = self.node.create_subscription(
Imu, '/imu/status', self._imu_status_callback, qos_profile)
# @timing_decorator
def get_imu_data(self):
"""处理传感器数据（非电机）"""
# 处理IMU数据
while True:
try:
msg = self.queue_imu_xsens.peek()
self.robot_data_.imu_data_[0] = msg.euler.yaw
self.robot_data_.imu_data_[1] = msg.euler.pitch
self.robot_data_.imu_data_[2] = msg.euler.roll
self.robot_data_.imu_data_[3] = msg.angular_velocity.x
self.robot_data_.imu_data_[4] = msg.angular_velocity.y
self.robot_data_.imu_data_[5] = msg.angular_velocity.z
self.robot_data_.imu_data_[6] = msg.linear_acceleration.x
self.robot_data_.imu_data_[7] = msg.linear_acceleration.y
self.robot_data_.imu_data_[8] = msg.linear_acceleration.z
self.robot_data_.imu_quat_[0] = msg.orientation.w
self.robot_data_.imu_quat_[1] = msg.orientation.x
self.robot_data_.imu_quat_[2] = msg.orientation.y
self.robot_data_.imu_quat_[3] = msg.orientation.z
break
except queue.Empty:
time.sleep(0.0001)
def update_robot_state(self):
"""读取电机状态数据更新为机器人状态数据"""
# 处理腿部电机状态
while True:
try:
msg = self.queue_leg_motor_state.peek()
if self.debug:
current_time = self.node.get_clock().now().to_msg()
msg_time = msg.header.stamp
time_diff = (current_time.sec -
msg_time.sec) * 1000000000 + (
current_time.nanosec - msg_time.nanosec)
time_diff_ms = time_diff / 1000000.0
print(f"Time difference: {time_diff_ms} ms")
for status in msg.status:
self.motor_state_to_robot_state(
status, self.first_leg_data_received)
break
except queue.Empty:
time.sleep(0.0001)
# 处理手臂电机状态
while True:
try:
msg = self.queue_arm_motor_state.peek()
if self.debug:
current_time = self.node.get_clock().now().to_msg()
msg_time = msg.header.stamp
time_diff = (current_time.sec -
msg_time.sec) * 1000000000 + (
current_time.nanosec - msg_time.nanosec)
time_diff_ms = time_diff / 1000000.0
print(f"Time difference: {time_diff_ms} ms")
for status in msg.status:
self.motor_state_to_robot_state(
status, self.first_arm_data_received)
break
except queue.Empty:
time.sleep(0.0001)
# 处理腰部电机状态
while True:
try:
msg = self.queue_waist_motor_state.peek()
if self.debug:
current_time = self.node.get_clock().now().to_msg()
msg_time = msg.header.stamp
time_diff = (current_time.sec -
msg_time.sec) * 1000000000 + (
current_time.nanosec - msg_time.nanosec)
time_diff_ms = time_diff / 1000000.0
print(f"Time difference: {time_diff_ms} ms")
for status in msg.status:
self.motor_state_to_robot_state(
status, self.first_waist_data_received)
break
except queue.Empty:
time.sleep(0.0001)
def motor_state_to_robot_state(self, status, received_flag: bool):
index = self.id_map.get_index_by_id(status.name)
if index >= 0:
robotdata_index = index + self.floating_base_dof # 偏移到完整关节数组中
# 直接赋值
self.robot_data_.q_a_[robotdata_index] = status.pos
self.robot_data_.q_dot_a_[robotdata_index] = status.speed
self.robot_data_.tau_a_[
robotdata_index] = status.current * self.ct_scale[min(
index,
len(self.ct_scale) - 1)]
self.robot_data_.temperature_a[
robotdata_index - self.floating_base_dof] = status.temperature
self.robot_data_.q_a_[robotdata_index] = (
status.pos - self.zero_pos[index]
) * self.motor_dir[index] + self.zero_offset[index]
if self.robot_data_.q_a_[robotdata_index] > math.pi:
self.zero_cnt[index] = -1.0
elif self.robot_data_.q_a_[robotdata_index] < -math.pi:
self.zero_cnt[index] = 1.0
self.robot_data_.q_a_[
robotdata_index] += self.zero_cnt[index] * 2.0 * math.pi
self.robot_data_.q_dot_a_[robotdata_index] *= self.motor_dir[index]
self.robot_data_.tau_a_[robotdata_index] *= self.motor_dir[index]
if not received_flag or abs(
self.robot_data_.q_a_[robotdata_index] -
self.robot_data_.q_a_last[robotdata_index]
) > self.joint_pos_threshold:
if received_flag:
self.node.get_logger().warn(
f"Joint {index} error detected")
self.robot_data_.q_a_[
robotdata_index] = self.robot_data_.q_a_last[
robotdata_index]
self.robot_data_.q_dot_a_[
robotdata_index] = self.robot_data_.qdot_a_last[
robotdata_index]
self.robot_data_.tau_a_[
robotdata_index] = self.robot_data_.tor_a_last[
robotdata_index]
else:
# 首次接收数据，更新标志位
received_flag = True
self.robot_data_.q_a_last[robotdata_index] = self.robot_data_.q_a_[
robotdata_index]
self.robot_data_.qdot_a_last[
robotdata_index] = self.robot_data_.q_dot_a_[robotdata_index]
self.robot_data_.tor_a_last[
robotdata_index] = self.robot_data_.tau_a_[robotdata_index]
def update_sensor_states(self):
# 获取Imu数据
self.get_imu_data()
# 添加IMU偏移
self.robot_data_.imu_data_[2] += self.xsense_roll_offset
def update_robot_data(self, flag: ControlFlag, time_passed: float):
# 更新传感器数据
self.update_sensor_states()
# 更新电机状态数据
self.update_robot_state()
# 更新机器人控制命令
self.update_robot_cmd(flag)
# 脚踝并联转串联
self.backup_serial_cmd()
if not self.sim:
#真机
self.ankle_parallel_to_serial()
# 更新robot_data 时间戳
self.robot_data_.time_now_ = time_passed
def backup_serial_cmd(self):
self.robot_data_.q_d_s_ = self.robot_data_.q_d_.copy()
def _check_and_clip_joint_limits_fast(
self, cmd_name: int, position: float) -> tuple[bool, float]:
"""
快速检查并修正关节位置限位（避免重复查询）
"""
if not self.sim and self.clip_actions:
limit = self.id_to_limits[cmd_name]
clipped_pos = np.clip(position, limit["min"], limit["max"])
return clipped_pos == position, clipped_pos
else:
return True, position
@timing_decorator
def convert_to_motor_commands(self):
"""将机器人控制命令转换为电机控制命令"""
# 使用切片操作一次性复制数据，避免逐个元素赋值
q_d_reordered = self.robot_data_.q_d_[self.floating_base_dof:]
qdot_d_reordered = self.robot_data_.q_dot_d_[self.floating_base_dof:]
tor_d_reordered = self.robot_data_.tau_d_[self.floating_base_dof:]
# 计算 q_d_reordered - self.zero_offset
np.subtract(q_d_reordered, self.zero_offset, out=self.temp_q)
# 计算 self.zero_cnt * 2.0 * self.pi
np.multiply(self.zero_cnt, 2.0 * math.pi, out=self._temp_zero_cnt)
# 计算 q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi
np.subtract(self.temp_q, self._temp_zero_cnt, out=self.temp_q)
# 计算 (q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi) * self.motor_dir
np.multiply(self.temp_q, self.motor_dir, out=self.temp_q)
# 计算最终结果 (q_d_reordered - self.zero_offset - self.zero_cnt * 2.0 * self.pi) * self.motor_dir + self.zero_pos
np.add(self.temp_q,
self.zero_pos,
out=self.robot_data_.q_d_[self.floating_base_dof:])
# qdot_d和tor_d的计算也可以向量化
np.multiply(qdot_d_reordered,
self.motor_dir,
out=self.robot_data_.q_dot_d_[self.floating_base_dof:])
np.multiply(tor_d_reordered,
self.motor_dir,
out=self.robot_data_.tau_d_[self.floating_base_dof:])
# 如果关节被禁用
if self.disable_joints_:
self.robot_data_.joint_kp_p_.fill(0.0)
self.robot_data_.joint_kd_p_.fill(0.0)
self.robot_data_.tau_d_.fill(0.0)
self.node.get_logger().warn("Joints disabled!")
def publish_motor_commands(self, flag: ControlFlag):
# flag_fsm_command = flag.fsm_state_command
current_state = self.current_state
if self.pub_fsm_state is not None:
state_msg = String()
state_msg.data = current_state.name
self.pub_fsm_state.publish(state_msg)
# 发布腿部控制命令
if self.legs_control_status == [] or current_state in self.legs_control_status:
leg_msg = CmdMotorCtrl()
leg_msg.header.stamp = self.node.get_clock().now().to_msg()
for i in range(self.id_map.leg_motor_nums):
cmd = MotorCtrl()
cmd.name = self.id_map.get_id_by_index(i)
cmd.kp = float(self.robot_data_.joint_kp_p_[i])
cmd.kd = float(self.robot_data_.joint_kd_p_[i])
cmd.pos = float(self.robot_data_.q_d_[i +
self.floating_base_dof])
cmd.spd = float(
self.robot_data_.q_dot_d_[i + self.floating_base_dof])
cmd.tor = float(
self.robot_data_.tau_d_[i + self.floating_base_dof])
# 检查关节位置限位
within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
cmd.name, cmd.pos)
if not within_limit:
print(
f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
)
leg_msg.cmds.append(cmd)
self.pub_leg_motor_cmd.publish(leg_msg)
# 只在特定模式下控制腰部
if current_state in self.waist_control_status:
# 腰部控制命令
waist_msg = CmdMotorCtrl()
waist_msg.header.stamp = self.node.get_clock().now().to_msg()
for i in range(self.id_map.waist_motor_nums):
cmd = MotorCtrl()
motor_idx = i + self.id_map.leg_motor_nums
cmd.name = self.id_map.get_id_by_index(
motor_idx) # 12 -> 33(yaw)
cmd.kp = float(self.robot_data_.joint_kp_p_[motor_idx])
cmd.kd = float(self.robot_data_.joint_kd_p_[motor_idx])
cmd.pos = float(self.robot_data_.q_d_[motor_idx +
self.floating_base_dof])
cmd.spd = float(
self.robot_data_.q_dot_d_[motor_idx +
self.floating_base_dof])
cmd.tor = float(
self.robot_data_.tau_d_[motor_idx +
self.floating_base_dof])
# 检查关节位置限位
within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
cmd.name, cmd.pos)
if not within_limit:
print(
f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
)
waist_msg.cmds.append(cmd)
# print(f'waist_msg {waist_msg}')
self.pub_waist_motor_cmd.publish(waist_msg)
# 只在特定模式下控制手臂
if current_state in self.arms_control_status:
# 手臂控制命令
arm_msg = CmdMotorCtrl()
arm_msg.header.stamp = self.node.get_clock().now().to_msg()
if current_state in self.left_arm_only_status:
control_index = np.arange(0, 7)
elif current_state in self.right_arm_only_status:
control_index = np.arange(self.id_map.arm_motor_nums - 7, self.id_map.arm_motor_nums)
else:
control_index = np.arange(0, self.id_map.arm_motor_nums)
for i in control_index:
cmd = MotorCtrl()
motor_idx = i + self.id_map.leg_motor_nums + self.id_map.waist_motor_nums
cmd.name = self.id_map.get_id_by_index(motor_idx)
cmd.kp = float(self.robot_data_.joint_kp_p_[motor_idx])
cmd.kd = float(self.robot_data_.joint_kd_p_[motor_idx])
cmd.pos = float(self.robot_data_.q_d_[motor_idx +
self.floating_base_dof])
cmd.spd = float(
self.robot_data_.q_dot_d_[motor_idx +
self.floating_base_dof])
cmd.tor = float(
self.robot_data_.tau_d_[motor_idx +
self.floating_base_dof])
# 检查关节位置限位
within_limit, cmd.pos = self._check_and_clip_joint_limits_fast(
cmd.name, cmd.pos)
if not within_limit:
print(
f"Joint (id: {cmd.name}) position {cmd.pos} is out of limits"
)
arm_msg.cmds.append(cmd)
# print(f'arm_msg {arm_msg}')
self.pub_arm_motor_cmd.publish(arm_msg)
@timing_decorator
def send_motor_commands(self, flag: ControlFlag):
"""发布电机控制命令"""
if not self.initialized:
return
if not self.sim:
#真机
self.ankle_serial_to_parallel()
self.convert_to_motor_commands()
self.publish_motor_commands(flag)
# ROS回调函数
def _leg_motor_status_callback(self, msg):
"""腿部电机状态回调"""
try:
self.queue_leg_motor_state.put_nowait(msg)
except queue.Full:
# 队列满时移除旧数据，加入新数据
try:
self.queue_leg_motor_state.get_nowait() # 移除旧数据
self.queue_leg_motor_state.put_nowait(msg) # 加入新数据
except:
pass # 如果仍然无法加入，忽略
def _arm_motor_status_callback(self, msg):
"""手臂电机状态回调"""
try:
self.queue_arm_motor_state.put_nowait(msg)
except queue.Full:
# 队列满时移除旧数据，加入新数据
try:
self.queue_arm_motor_state.get_nowait() # 移除旧数据
self.queue_arm_motor_state.put_nowait(msg) # 加入新数据
except:
pass # 如果仍然无法加入，忽略
def _waist_motor_status_callback(self, msg):
"""腰部电机状态回调"""
try:
self.queue_waist_motor_state.put_nowait(msg)
except queue.Full:
# 队列满时移除旧数据，加入新数据
try:
self.queue_waist_motor_state.get_nowait() # 移除旧数据
self.queue_waist_motor_state.put_nowait(msg) # 加入新数据
except:
pass # 如果仍然无法加入，忽略
def _imu_status_callback(self, msg):
"""IMU状态回调"""
try:
self.queue_imu_xsens.put_nowait(msg)
except queue.Full:
try:
self.queue_imu_xsens.get_nowait() # 移除旧数据
self.queue_imu_xsens.put_nowait(msg) # 加入新数据
except:
pass # 如果仍然无法加入，忽略
def ankle_parallel_to_serial(self):
# 串并联转换：并转串 (类似C++版本中的处理)
# 提取左右脚两个踝关节（并联关节）
q_a_p = np.zeros(4) # 并联关节角度（实际）
qdot_a_p = np.zeros(4) # 并联关节速度（实际）
tor_a_p = np.zeros(4) # 并联关节力矩（实际）
q_a_s = np.zeros(4) # 串联关节角度（实际）
qdot_a_s = np.zeros(4) # 串联关节速度（实际）
tor_a_s = np.zeros(4) # 串联关节力矩（实际）
q_a_p[:2] = self.robot_data_.q_a_[
self.left_ankle_indices] # 左脚踝关节 (pitch, roll)
q_a_p[2:] = self.robot_data_.q_a_[
self.right_ankle_indices] # 右脚踝关节 (pitch, roll)
qdot_a_p[:2] = self.robot_data_.q_dot_a_[self.left_ankle_indices]
qdot_a_p[2:] = self.robot_data_.q_dot_a_[self.right_ankle_indices]
tor_a_p[:2] = self.robot_data_.tau_a_[self.left_ankle_indices]
tor_a_p[2:] = self.robot_data_.tau_a_[self.right_ankle_indices]
self.q_a_p = q_a_p.copy()
self.qdot_a_p = qdot_a_p.copy()
self.tor_a_p = tor_a_p.copy()
if self.debug:
print("-" * 20 + "并转串联前" + "-" * 20)
print("q_a_p:", q_a_p)
print("qdot_a_p:", qdot_a_p)
print("tor_a_p:", tor_a_p)
# 计算并转串（正运动学）
self.fun_s2p.set_p_est(q_a_p, qdot_a_p, tor_a_p)
self.fun_s2p.calcFK()
self.fun_s2p.calcIK()
success, q_a_s, qdot_a_s, tor_a_s = self.fun_s2p.get_s_state()
if self.debug:
print("-" * 20 + "并转串联后" + "-" * 20)
print("q_a_s:", q_a_s)
print("qdot_a_s:", qdot_a_s)
print("tor_a_s:", tor_a_s)
# 用串联关节值替换原来的并联关节值
self.robot_data_.q_a_[self.left_ankle_indices] = q_a_s[:2] # 左脚踝关节串联值
self.robot_data_.q_a_[self.right_ankle_indices] = q_a_s[2:] # 右脚踝关节串联值
self.robot_data_.q_dot_a_[self.left_ankle_indices] = qdot_a_s[:2]
self.robot_data_.q_dot_a_[self.right_ankle_indices] = qdot_a_s[2:]
self.robot_data_.tau_a_[self.left_ankle_indices] = tor_a_s[:2]
self.robot_data_.tau_a_[self.right_ankle_indices] = tor_a_s[2:]
def ankle_serial_to_parallel(self):
# 串转并：将串联关节命令转换为并联关节命令（类似C++版本）
# 提取踝关节两关节的串联命令
q_d_p = np.zeros(4) # 并联关节角度（期望）
qdot_d_p = np.zeros(4) # 并联关节速度（期望）
tor_d_p = np.zeros(4) # 并联关节力矩（期望）
q_d_s = np.zeros(4) # 串联关节角度（期望）
qdot_d_s = np.zeros(4) # 串联关节速度（期望）
tor_d_s = np.zeros(4) # 串联关节力矩（期望）
q_d_s[:2] = self.robot_data_.q_d_[self.left_ankle_indices] # 左脚踝关节串联命令
q_d_s[2:] = self.robot_data_.q_d_[
self.right_ankle_indices] # 右脚踝关节串联命令
qdot_d_s[:2] = self.robot_data_.q_dot_d_[self.left_ankle_indices]
qdot_d_s[2:] = self.robot_data_.q_dot_d_[self.right_ankle_indices]
tor_d_s[:2] = self.robot_data_.tau_d_[self.left_ankle_indices]
tor_d_s[2:] = self.robot_data_.tau_d_[self.right_ankle_indices]
q_a_s = np.zeros(4) # 串联关节角度（实际）
qdot_a_s = np.zeros(4) # 串联关节速度（实际）
q_a_s[:2] = self.robot_data_.q_a_[self.left_ankle_indices] # 左脚踝关节串联值
q_a_s[2:] = self.robot_data_.q_a_[self.right_ankle_indices] # 右脚踝关节串联值
qdot_a_s[:2] = self.robot_data_.q_dot_a_[
self.left_ankle_indices] # 左脚踝关节串联速度
qdot_a_s[2:] = self.robot_data_.q_dot_a_[
self.right_ankle_indices] # 右脚踝关节串联速度
kp = np.zeros(4) # 串联关节刚度
kd = np.zeros(4) # 串联关节阻尼
kp[:2] = self.robot_data_.joint_kp_p_[self.left_ankle_indices -
self.floating_base_dof]
kp[2:] = self.robot_data_.joint_kp_p_[self.right_ankle_indices -
self.floating_base_dof]
kd[:2] = self.robot_data_.joint_kd_p_[self.left_ankle_indices -
self.floating_base_dof]
kd[2:] = self.robot_data_.joint_kd_p_[self.right_ankle_indices -
self.floating_base_dof]
tor_d_s = kp * (q_d_s - q_a_s) + kd * (qdot_d_s - qdot_a_s)
if self.debug:
print("-" * 20 + "串转并联前" + "-" * 20)
print("q_d_s:", q_d_s)
print("qdot_d_s:", qdot_d_s)
print("tor_d_s:", tor_d_s)
# 串转并计算
self.fun_s2p.set_s_des(q_d_s, qdot_d_s, tor_d_s)
self.fun_s2p.calc_joint_pos_ref()
self.fun_s2p.calc_joint_tor_des()
success, q_d_p, qdot_d_p, tor_d_p = self.fun_s2p.get_p_des()
q_d_p = (tor_d_p - self.ankle_kd_p *
(qdot_d_p - self.qdot_a_p)) / self.ankle_kp_p + self.q_a_p
if self.debug:
print("-" * 20 + "串转并联后" + "-" * 20)
print("q_d_p:", q_d_p)
print("qdot_d_p:", qdot_d_p)
print("tor_d_p:", tor_d_p)
# 用并联关节命令覆盖原来的串联命令
self.robot_data_.q_d_[self.left_ankle_indices] = q_d_p[:2] # 左脚踝关节并联命令
self.robot_data_.q_d_[self.right_ankle_indices] = q_d_p[2:] # 右脚踝关节并联命令
self.robot_data_.q_dot_d_[self.left_ankle_indices] = qdot_d_p[:2]
self.robot_data_.q_dot_d_[self.right_ankle_indices] = qdot_d_p[2:]
# self.robot_data_.tau_d_[self.left_ankle_indices] = tor_d_p[:2]
# self.robot_data_.tau_d_[self.right_ankle_indices] = tor_d_p[2:]
self.robot_data_.tau_d_[self.left_ankle_indices] = 0.0
self.robot_data_.tau_d_[self.right_ankle_indices] = 0.0
# 更新脚踝Kp,Kd
self.robot_data_.joint_kp_p_[
self.left_ankle_indices -
self.floating_base_dof] = self.ankle_kp_p[:2]
self.robot_data_.joint_kp_p_[
self.right_ankle_indices -
self.floating_base_dof] = self.ankle_kp_p[2:]
self.robot_data_.joint_kd_p_[
self.left_ankle_indices -
self.floating_base_dof] = self.ankle_kd_p[:2]
self.robot_data_.joint_kd_p_[
self.right_ankle_indices -
self.floating_base_dof] = self.ankle_kd_p[2:]
def update_robot_cmd(self, flag: ControlFlag):
"""更新机器人控制命令"""
if flag.enable:
# 使用getattr设置默认值，避免AttributeError
x_command = getattr(flag, 'x_speed_command')
y_command = getattr(flag, 'y_speed_command')
yaw_command = getattr(flag, 'yaw_speed_command')
self.robot_data_.walk_cmd_ = [x_command, y_command, yaw_command]
self.robot_data_.control_flag.fsm_state_command = flag.fsm_state_command
def get_robot_interface(robot_data: RobotData, config_path: str) -> RobotInterface:
"""工厂函数，返回机器人接口实例"""
return RobotInterfaceImpl(robot_data, config_path)