815 lines
34 KiB
Python
815 lines
34 KiB
Python
"""
|
||
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)
|