中间层力控开发初步完成，待测试
This commit is contained in:
ziwei.he
parent
333aa7a56a
commit
4bf67089fe
@ -1,8 +1,10 @@
|
||||
from hardware.dobot_nova5 import dobot_nova5
|
||||
from hardware.force_sensor import XjcSensor
|
||||
|
||||
from algorithms.arm_state import ArmState
|
||||
from algorithms.controller_manager import ControllerManager
|
||||
from algorithms.admittance_controller import AdmittanceController
|
||||
from algorithms.position_controller import PositionController
|
||||
|
||||
from tools.log import CustomLogger
|
||||
from tools.yaml_operator import read_yaml
|
||||
@ -11,6 +13,7 @@ import numpy as np
|
||||
import threading
|
||||
import time
|
||||
import os
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
class MassageRobot:
|
||||
def __init__(self,arm_config_path,is_log=False):
|
||||
self.logger = CustomLogger(
|
||||
@ -26,14 +29,16 @@ class MassageRobot:
|
||||
|
||||
self.arm_state = ArmState()
|
||||
self.arm_config = read_yaml(arm_config_path)
|
||||
# arm 实例化时机械臂类内部进行通讯连接
|
||||
self.arm = dobot_nova5(arm_ip=self.arm_config['arm_ip'])
|
||||
self.force_sensor = None
|
||||
self.force_sensor = XjcSensor(host=self.arm_config['arm_ip'],port=60000)
|
||||
|
||||
# controller
|
||||
# 控制器初始化(初始化为导纳控制)
|
||||
self.controller_manager = ControllerManager(self.arm_state)
|
||||
self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
|
||||
|
||||
# massage heads
|
||||
self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
|
||||
self.controller_manager.switch_controller('admittance')
|
||||
# 按摩头参数暂时使用本地数据
|
||||
massage_head_dir = self.arm_config['massage_head_dir']
|
||||
all_items = os.listdir(massage_head_dir)
|
||||
head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
|
||||
@ -44,7 +49,7 @@ class MassageRobot:
|
||||
self.playload_dict[play_load['name']] = play_load
|
||||
self.current_head = 'none'
|
||||
|
||||
# 频率
|
||||
# 频率数据赋值
|
||||
self.control_rate = Rate(self.arm_config['control_rate'])
|
||||
self.sensor_rate = Rate(self.arm_config['sensor_rate'])
|
||||
self.command_rate = Rate(self.arm_config['command_rate'])
|
||||
@ -52,15 +57,20 @@ class MassageRobot:
|
||||
# 低通滤波
|
||||
self.cutoff_freq = 80.0
|
||||
|
||||
# flags
|
||||
# 停止标志位线程
|
||||
self.exit_event = threading.Event()
|
||||
self.exit_event.set() # 运行 True
|
||||
self.exit_event.set() # 运行初始化为True
|
||||
self.interrupt_event = threading.Event()
|
||||
self.interrupt_event.clear() # 中断 False
|
||||
self.interrupt_event.clear() # 中断初始化为False
|
||||
self.pause_envent = threading.Event()
|
||||
self.pause_envent.clear() # 暂停 False
|
||||
self.pause_envent.clear() # 暂停初始化为False
|
||||
self.skip_envent = threading.Event()
|
||||
self.skip_envent.clear() # 跳过 False
|
||||
self.skip_envent.clear() # 跳过初始化为False
|
||||
# 按摩调整
|
||||
self.adjust_wrench_event = threading.Event()
|
||||
self.adjust_wrench_event.clear() # 调整初始化为False
|
||||
self.pos_increment = np.zeros(3,dtype=np.float64)
|
||||
self.adjust_wrench = np.zeros(6,dtype=np.float64)
|
||||
|
||||
self.is_waitting = False
|
||||
|
||||
@ -68,8 +78,7 @@ class MassageRobot:
|
||||
self.last_record_time = 0
|
||||
self.last_command_time = 0
|
||||
self.move_to_point_count = 0
|
||||
self.width_default = 5
|
||||
|
||||
# 卡尔曼滤波相关初始化
|
||||
self.x_base = np.zeros(6)
|
||||
self.P_base = np.eye(6)
|
||||
# 过程噪声协方差矩阵
|
||||
@ -86,8 +95,8 @@ class MassageRobot:
|
||||
# 传感器故障计数器
|
||||
self.sensor_fail_count = 0
|
||||
|
||||
# init
|
||||
self.arm.__init__()
|
||||
# 机械臂初始化,适配中间层
|
||||
self.arm.init()
|
||||
|
||||
# 预测步骤
|
||||
def kalman_predict(self,x, P, Q):
|
||||
@ -107,14 +116,101 @@ class MassageRobot:
|
||||
P_update = (np.eye(len(K)) - K) @ P_predict
|
||||
return x_update, P_update
|
||||
|
||||
def update_wrench(self):
|
||||
compensation_config = self.playload_dict[self.current_head]
|
||||
# 读取数据
|
||||
gravity_base = np.array(compensation_config['gravity_base'])
|
||||
force_zero = np.array(compensation_config['force_zero'])
|
||||
torque_zero = np.array(compensation_config['torque_zero'])
|
||||
tool_position = np.array(compensation_config['tcp_offset'])
|
||||
mass_center_position = np.array(compensation_config['mass_center_position'])
|
||||
# 当前的机械臂到末端转换 (实时)
|
||||
b_rotation_s = R.from_quat(self.arm_state.arm_orientation).as_matrix()
|
||||
# 读取数据
|
||||
sensor_data = self.force_sensor.read()
|
||||
if sensor_data is None:
|
||||
self.force_sensor.stop_background_reading()
|
||||
self.logger.log_error("传感器数据读取失败")
|
||||
return -1
|
||||
# 传感器数据通过矫正计算得到外来施加力 传感器坐标系下
|
||||
# 重力和零位
|
||||
gravity_in_sensor = b_rotation_s.T @ gravity_base
|
||||
s_force = sensor_data[:3] - force_zero - gravity_in_sensor
|
||||
# 力矩
|
||||
s_torque = sensor_data[3:] - torque_zero - np.cross(mass_center_position,gravity_in_sensor)
|
||||
wrench = np.concatenate([s_force,s_torque])
|
||||
# 传感器工具转换
|
||||
s_tf_matrix_t = self.get_tf_matrix(tool_position[:3], R.from_euler('xyz',tool_position[3:],degrees=False).as_quat())
|
||||
# 传感器到TCP
|
||||
wrench = self.wrench_coordinate_conversion(s_tf_matrix_t,wrench)
|
||||
# 交给ARM STATE集中管理
|
||||
self.arm_state.external_wrench_tcp = wrench
|
||||
self.arm_state.external_wrench_base = np.concatenate([b_rotation_s @ self.arm_state.external_wrench_tcp[:3],
|
||||
b_rotation_s @ self.arm_state.external_wrench_tcp[3:]])
|
||||
# 卡尔曼滤波
|
||||
x_base_predict, P_base_predict = self.kalman_predict(x = self.x_base,
|
||||
P = self.P_base,
|
||||
Q = self.Q_base)
|
||||
self.x_base, self.P_base = self.kalman_update(x_predict = x_base_predict,
|
||||
P_predict = P_base_predict,
|
||||
z = self.arm_state.external_wrench_base,
|
||||
R = self.R_base)
|
||||
self.arm_state.external_wrench_base = self.x_base
|
||||
self.arm_state.last_external_wrench_base = self.arm_state.external_wrench_base
|
||||
# 对tcp坐标系下的外力外矩进行平滑
|
||||
x_tcp_predict, P_tcp_predict = self.kalman_predict(x = self.x_tcp,
|
||||
P = self.P_tcp,
|
||||
Q = self.Q_tcp)
|
||||
self.x_tcp, self.P_tcp = self.kalman_update(x_predict = x_tcp_predict,
|
||||
P_predict = P_tcp_predict,
|
||||
z = self.arm_state.external_wrench_tcp,
|
||||
R = self.R_tcp)
|
||||
self.arm_state.external_wrench_tcp = self.x_tcp
|
||||
self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
|
||||
return 0
|
||||
|
||||
def arm_measure_loop(self):
|
||||
return
|
||||
self.logger.log_info("机械臂测量线程启动")
|
||||
while (not self.arm.is_exit) and (not self.exit_event.is_set()):
|
||||
try:
|
||||
if not self.is_waitting:
|
||||
self.arm_state.arm_position,
|
||||
self.arm_state.arm_orientation = self.arm.get_arm_position()
|
||||
code = self.update_wrench()
|
||||
if code == -1:
|
||||
self.sensor_fail_count += 1
|
||||
self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}")
|
||||
if self.sensor_fail_count > 10:
|
||||
self.logger.log_error("传感器线程数据读取失败超过10次,程序终止")
|
||||
self.stop()
|
||||
break
|
||||
else:
|
||||
self.sensor_fail_count = 0
|
||||
except Exception as e:
|
||||
self.logger.log_error(f"机械臂测量线程报错:{e}")
|
||||
self.exit_event.set() # 控制退出while
|
||||
self.sensor_rate.precise_sleep() # 控制频率
|
||||
|
||||
def arm_command_loop(self):
|
||||
return
|
||||
|
||||
self.logger.log_info("机械臂控制线程启动")
|
||||
while (not self.arm.is_exit) and (not self.exit_event.is_set()):
|
||||
try:
|
||||
if not self.is_waitting:
|
||||
self.controller_manager.step(self.control_rate.to_sec())
|
||||
self.last_command_time += 1
|
||||
code = self.arm.ServoPose(self.arm_state.arm_position,
|
||||
self.arm_state.arm_orientation_command)
|
||||
if code == -1:
|
||||
self.logger.log_error("机械臂急停")
|
||||
# self.stop() # 底层已经做了急停处理
|
||||
break
|
||||
except Exception as e:
|
||||
self.logger.log_error(f"机械臂控制失败:{e}")
|
||||
self.exit_event.set()
|
||||
self.control_rate.precise_sleep()
|
||||
def start(self):
|
||||
if self.exit_event.is_set():
|
||||
# 线程
|
||||
self.exit_event.clear()
|
||||
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
|
||||
self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
|
||||
@ -125,26 +221,48 @@ class MassageRobot:
|
||||
self.arm_state.arm_position_command = position
|
||||
self.arm_state.desired_orientation = quat_rot
|
||||
self.arm_state.arm_orientation_command = quat_rot
|
||||
self.logger.log_info("MassageRobot启动")
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
|
||||
def stop(self):
|
||||
if not self.exit_event.is_set():
|
||||
self.exit_event.set()
|
||||
self.interrupt_event.clear()
|
||||
self.arm_control_thread.join()
|
||||
self.arm_measure_thread.join()
|
||||
for i in range(3):
|
||||
self.force_sensor.disable_active_transmission()
|
||||
self.force_sensor.start_background_reading()
|
||||
self.arm.stop_motion()
|
||||
self.logger.log_info("MassageRobot停止")
|
||||
|
||||
return
|
||||
|
||||
def init_hardwares(self,ready_pose):
|
||||
self.ready_pose = np.array(ready_pose)
|
||||
self.switch_payload(self.current_head)
|
||||
self.arm_state.desired_orientation = self.ready_pose[:3]
|
||||
euler_angles = self.ready_pose
|
||||
self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat()
|
||||
time.sleep(0.5)
|
||||
|
||||
def switch_payload(self,name):
|
||||
if name in self.playload_dict:
|
||||
return
|
||||
|
||||
self.stop()
|
||||
self.current_head = name
|
||||
tcp_offset = self.playload_dict[name]["tcp_offset"]
|
||||
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
|
||||
print(tcp_offset_str)
|
||||
self.arm.setEndEffector(i=1,tool_i=tcp_offset_str)
|
||||
self.arm.chooseEndEffector(i=1)
|
||||
self.logger.log_info(f"切换到{name}按摩头")
|
||||
R_matrix = R.from_euler('xyz',self.ready_pose[3:] ,degrees=False).as_matrix()
|
||||
ready_position = self.ready_pose[:3] + R_matrix @ self.playload_dict[self.current_head]['tcp_offset'][:3]
|
||||
ready_orientation = R_matrix @ R.from_euler('xyz',self.playload_dict[self.current_head]['tcp_offset'][3:],degrees=False).as_matrix()
|
||||
ready_orientation_euler = R.from_matrix(ready_orientation).as_euler('xyz',degrees=False)
|
||||
self.arm_state.desired_position = ready_position
|
||||
self.arm_state.desired_orientation = R.from_euler('xyz',ready_orientation_euler,degrees=False).as_quat()
|
||||
self.controller_manager.switch_controller('position')
|
||||
else:
|
||||
self.logger.log_error(f"未找到{name}按摩头")
|
||||
|
||||
def log_thread(self):
|
||||
while True:
|
||||
@ -155,5 +273,21 @@ class MassageRobot:
|
||||
self.logger_data.log_info(f"当前按摩头:{self.current_head}",is_print=False)
|
||||
time.sleep(1)
|
||||
|
||||
# 工具函数
|
||||
def get_tf_matrix(position,orientation):
|
||||
tf_matrix = np.eye(4)
|
||||
rotation_matrix = R.from_quat(orientation).as_matrix()
|
||||
tf_matrix[:3,3] = position
|
||||
tf_matrix[:3,:3] = rotation_matrix
|
||||
return tf_matrix
|
||||
def wrench_coordinate_conversion(tf_matrix, wrench):
|
||||
rot_matrix = tf_matrix[:3, :3]
|
||||
vector_p = tf_matrix[:3, 3]
|
||||
temp_force = wrench[:3]
|
||||
torque = wrench[3:]
|
||||
force = rot_matrix.T @ temp_force
|
||||
torque = rot_matrix.T @ np.cross(temp_force,vector_p) + rot_matrix.T @ torque
|
||||
return np.concatenate([force, torque])
|
||||
|
||||
if __name__ == "__main__":
|
||||
robot = MassageRobot(arm_config_path="MassageControl/config/robot_config.yaml")
|
||||
68
Massage/MassageControl/algorithms/position_controller.py
Normal file
68
Massage/MassageControl/algorithms/position_controller.py
Normal file
@ -0,0 +1,68 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from .base_controller import BaseController
|
||||
from .arm_state import ArmState
|
||||
from pathlib import Path
|
||||
import time
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from tools.yaml_operator import read_yaml
|
||||
|
||||
class PositionController(BaseController):
|
||||
def __init__(self, name, state:ArmState,config_path) -> None:
|
||||
super().__init__(name, state)
|
||||
self.load_config(config_path)
|
||||
|
||||
self.laset_print_time = 0
|
||||
|
||||
def load_config(self, config_path):
|
||||
config_dict = read_yaml(config_path)
|
||||
if self.name != config_dict['name']:
|
||||
raise ValueError(f"Controller name {self.name} does not match config name {config_dict['name']}")
|
||||
self.Kp = np.diag(np.array(config_dict['Kp']))
|
||||
self.Ki = np.diag(np.array(config_dict['Ki']))
|
||||
self.Kd = np.diag(np.array(config_dict['Kd']))
|
||||
|
||||
self.pose_integral_error = np.zeros(6)
|
||||
|
||||
def step(self,dt):
|
||||
# print(f"desired position: {self.state.desired_position}, desired orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=False)}")
|
||||
# print(f"current position: {self.state.arm_position}, current orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=False)}")
|
||||
self.state.pose_error[:3] = self.state.arm_position - self.state.desired_position
|
||||
if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
||||
self.state.arm_orientation = -self.state.arm_orientation
|
||||
rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
|
||||
rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
|
||||
self.state.pose_error[3:] = rot_err_rotvex
|
||||
|
||||
self.pose_integral_error += self.state.pose_error * dt
|
||||
|
||||
self.state.arm_desired_acc = - self.Kd @ (self.state.arm_desired_twist - self.state.desired_twist) - self.Kp @ self.state.pose_error - self.Ki @ self.pose_integral_error
|
||||
self.clip_command(self.state.arm_desired_acc,"acc")
|
||||
self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
|
||||
self.clip_command(self.state.arm_desired_twist,"vel")
|
||||
delta_pose = self.state.arm_desired_twist * dt
|
||||
self.clip_command(delta_pose,"pose")
|
||||
delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
|
||||
arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
|
||||
|
||||
self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
|
||||
# 归一化四元数
|
||||
self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
|
||||
self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
|
||||
|
||||
# if time.time() - self.laset_print_time > 0.5:
|
||||
# print("---------------positioner-------------------------------------")
|
||||
# print("arm_position:",self.state.arm_position)
|
||||
# print("desired_position:",self.state.desired_position)
|
||||
# print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True))
|
||||
# print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True))
|
||||
# print("arm_position_command",self.state.arm_position_command)
|
||||
# print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True))
|
||||
# print("delta_pose:",delta_pose)
|
||||
# self.laset_print_time = time.time()
|
||||
|
||||
# if time.time() - self.last_print_time > 1:
|
||||
# print(self.state.arm_position_command,R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True))
|
||||
# self.last_print_time = time.time()
|
||||
@ -125,6 +125,7 @@ class dobot_nova5:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_P_inJoint(self,joints_list):
|
||||
'''
|
||||
@ -160,6 +161,7 @@ class dobot_nova5:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1) # 避免 CPU 占用过高
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inPose(self,poses_list):
|
||||
'''
|
||||
@ -188,6 +190,7 @@ class dobot_nova5:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inJoint(self,joints_list):
|
||||
'''
|
||||
@ -216,6 +219,7 @@ class dobot_nova5:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def ServoJoint(self,joints_list,t=-1,aheadtime=-1,gain=-1):
|
||||
tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t={t},aheadtime={aheadtime},gain={gain})"
|
||||
@ -225,12 +229,14 @@ class dobot_nova5:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
return -1
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
return 0
|
||||
|
||||
def ServoPose(self,poses_list,t=-1,aheadtime=-1,gain=-1):
|
||||
poses_list = list(map(float, poses_list))
|
||||
@ -427,12 +433,40 @@ class dobot_nova5:
|
||||
else:
|
||||
print("拖拽模式关闭失败")
|
||||
return
|
||||
|
||||
def stop_motion(self):
|
||||
res = self.dashboard.Stop()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂停止下发运动指令队列")
|
||||
else:
|
||||
print("停止下发运动失败,需立即拍下急停")
|
||||
return
|
||||
|
||||
def __del__(self):
|
||||
del self.dashboard
|
||||
del self.feedFour
|
||||
|
||||
# 适配中间层接口
|
||||
## 适配中间层的再封装接口
|
||||
|
||||
# 为状态管理而封装的初始化函数
|
||||
def init(self):
|
||||
self.is_exit = False
|
||||
self.traj_list = None
|
||||
self.last_pose_command = np.zeros(6)
|
||||
self.last_input_command = None
|
||||
self.tcp_offset = None
|
||||
self.init_J = [0,30,-120,0,90,0]
|
||||
sleep(1)
|
||||
self.is_standby = False
|
||||
code = self.RunPoint_P_inJoint(self.init_J)
|
||||
if code == 0:
|
||||
print("机械臂初始化成功且到达初始位置")
|
||||
else:
|
||||
print("机械臂初始化失败")
|
||||
|
||||
|
||||
|
||||
def get_arm_position(self):
|
||||
pose = self.getPose()
|
||||
x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
|
||||
@ -442,6 +476,7 @@ class dobot_nova5:
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
dobot = dobot_nova5("192.168.5.1")
|
||||
dobot.start()
|
||||
|
||||
@ -0,0 +1,572 @@
|
||||
import time
|
||||
import socket
|
||||
import struct
|
||||
import numpy as np
|
||||
import atexit
|
||||
import threading
|
||||
from typing import Optional, Tuple
|
||||
from matplotlib import pyplot as plt
|
||||
import csv
|
||||
from datetime import datetime
|
||||
|
||||
class XjcSensor:
|
||||
def __init__(self, host: str, port: int, rate: int = 250):
|
||||
"""
|
||||
初始化 TCP 连接的力传感器
|
||||
:param host: 设备 IP 地址（如 "192.168.1.100"）
|
||||
:param port: 设备端口（如 502）
|
||||
:param rate: 数据采样率（100/250/500Hz）
|
||||
"""
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.rate = rate
|
||||
self.slave_address = 0x01 # 默认从机地址
|
||||
self.sock = None # TCP socket 对象
|
||||
self.crc16_table = self.generate_crc16_table()
|
||||
# 后台读取相关的属性
|
||||
self._background_thread = None
|
||||
self._stop_background = False
|
||||
self._last_reading = None
|
||||
self._reading_lock = threading.Lock()
|
||||
# 建立 TCP 连接
|
||||
self.connect()
|
||||
|
||||
# 注册退出时的清理函数
|
||||
atexit.register(self.disconnect)
|
||||
|
||||
def __new__(cls, *args, **kwargs):
|
||||
"""单例模式"""
|
||||
if not hasattr(cls, 'instance'):
|
||||
cls.instance = super(XjcSensor, cls).__new__(cls)
|
||||
return cls.instance
|
||||
|
||||
def connect(self):
|
||||
"""建立 TCP 连接"""
|
||||
try:
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.sock.settimeout(1.0) # 设置超时时间
|
||||
self.sock.connect((self.host, self.port))
|
||||
print(f"Connected to {self.host}:{self.port}")
|
||||
except Exception as e:
|
||||
print(f"TCP connection failed: {e}")
|
||||
raise
|
||||
|
||||
def send_and_receive(self, data: bytes, expected_response_len: int) -> Optional[bytes]:
|
||||
"""
|
||||
发送数据并接收响应
|
||||
:param data: 要发送的字节数据
|
||||
:param expected_response_len: 预期响应的长度
|
||||
:return: 收到的字节数据（失败返回 None）
|
||||
"""
|
||||
try:
|
||||
self.sock.sendall(data)
|
||||
response = self.sock.recv(expected_response_len)
|
||||
return response
|
||||
except socket.timeout:
|
||||
print("Timeout while waiting for response")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"TCP communication error: {e}")
|
||||
return None
|
||||
|
||||
def set_zero(self) -> int:
|
||||
"""传感器置零（TCP 版本）"""
|
||||
if self.slave_address == 0x01:
|
||||
zero_cmd = bytes.fromhex('01 10 46 1C 00 02 04 00 00 00 00 E8 95')
|
||||
elif self.slave_address == 0x09:
|
||||
zero_cmd = bytes.fromhex('09 10 46 1C 00 02 04 00 00 00 00 C2 F5')
|
||||
|
||||
response = self.send_and_receive(zero_cmd, 8)
|
||||
if not response:
|
||||
print("set zero fail")
|
||||
return -1
|
||||
|
||||
# CRC 校验（假设协议要求）
|
||||
hi_check, lo_check = self.crc16(response[:-2])
|
||||
if (response[0] != self.slave_address or
|
||||
response[1] != 0x10 or
|
||||
response[-2] != lo_check or
|
||||
response[-1] != hi_check):
|
||||
print("Set zero failed: CRC check error")
|
||||
return -1
|
||||
|
||||
print("Set zero success!")
|
||||
return 0
|
||||
|
||||
def read_data_f32(self) -> np.ndarray:
|
||||
"""
|
||||
TCP版本 - 读取六维力传感器数据 (float32格式)
|
||||
返回:
|
||||
np.ndarray(6,) - 六维力数据 [Fx,Fy,Fz,Mx,My,Mz]
|
||||
读取失败返回 -1
|
||||
"""
|
||||
# 构造读取命令
|
||||
|
||||
if self.slave_address == 0x01:
|
||||
command = bytes.fromhex('01 04 00 00 00 0C F0 0F')
|
||||
elif self.slave_address == 0x09:
|
||||
command = bytes.fromhex('09 04 00 54 00 0C B0 97')
|
||||
|
||||
try:
|
||||
# 清空接收缓冲区
|
||||
self._clear_tcp_buffer()
|
||||
|
||||
# 接收完整响应
|
||||
response = self.send_and_receive(command, 29)
|
||||
|
||||
# CRC校验
|
||||
Hi_check, Lo_check = self.crc16(response[:-2])
|
||||
if (response[0] != self.slave_address or
|
||||
response[1] != 0x04 or
|
||||
response[2] != 24 or # 数据长度字节
|
||||
response[-2] != Lo_check or
|
||||
response[-1] != Hi_check):
|
||||
print(f"Protocol error! Received: {response.hex(' ')}")
|
||||
print(f"Expected slave:{self.slave_address}, "
|
||||
f"func:0x04, len:24, "
|
||||
f"CRC:{Lo_check:02X}{Hi_check:02X}")
|
||||
return -1
|
||||
|
||||
# 解析6个float32数据 (大端序)
|
||||
sensor_data = struct.unpack('>ffffff', response[3:27])
|
||||
return np.array(sensor_data)
|
||||
|
||||
except socket.timeout:
|
||||
print("Timeout while waiting for sensor response")
|
||||
return -1
|
||||
except ConnectionError as e:
|
||||
print(f"TCP connection error: {e}")
|
||||
return -1
|
||||
except Exception as e:
|
||||
print(f"Unexpected error in read_data_f32: {str(e)}")
|
||||
return -1
|
||||
|
||||
def _clear_tcp_buffer(self):
|
||||
"""清空TCP接收缓冲区"""
|
||||
self.sock.settimeout(0.1) # 短暂超时，避免阻塞
|
||||
try:
|
||||
while True:
|
||||
data = self.sock.recv(1024)
|
||||
if not data: # 连接关闭或无数据
|
||||
break
|
||||
print(f"Cleared TCP buffer: {data.hex(' ')}") # 调试输出
|
||||
except socket.timeout:
|
||||
pass # 预期内的超时，表示缓冲区已空
|
||||
finally:
|
||||
self.sock.settimeout(2.0) # 恢复默认超时
|
||||
|
||||
def enable_active_transmission(self) -> int:
|
||||
"""启用主动传输模式（TCP 版本）"""
|
||||
if self.rate == 100:
|
||||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 00 AB 6A') if self.slave_address == 0x01 else \
|
||||
bytes.fromhex('09 10 01 9A 00 01 02 00 00 CC AA')
|
||||
elif self.rate == 250:
|
||||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 01 6A AA') if self.slave_address == 0x01 else \
|
||||
bytes.fromhex('09 10 01 9A 00 01 02 00 01 0D 6A')
|
||||
elif self.rate == 500:
|
||||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 02 2A AB') if self.slave_address == 0x01 else \
|
||||
bytes.fromhex('09 10 01 9A 00 01 02 00 02 4D 6B')
|
||||
else:
|
||||
print("Unsupported rate")
|
||||
return -1
|
||||
|
||||
response = self.send_and_receive(cmd, 8)
|
||||
if response and response[1] == 0x10: # 检查响应功能码
|
||||
print(f"Active transmission enabled at {self.rate}Hz")
|
||||
return 0
|
||||
else:
|
||||
print("Failed to enable active transmission")
|
||||
return -1
|
||||
|
||||
def disconnect(self):
|
||||
"""关闭 TCP 连接"""
|
||||
if self.sock:
|
||||
try:
|
||||
self.sock.close()
|
||||
print("TCP connection closed")
|
||||
except Exception as e:
|
||||
print(f"Error while closing socket: {e}")
|
||||
self.sock = None
|
||||
def start_background_reading(self):
|
||||
"""
|
||||
启动后台读取任务，每秒读取一次传感器数据
|
||||
"""
|
||||
if self._background_thread is not None and self._background_thread.is_alive():
|
||||
print("Background reading is already running")
|
||||
return False
|
||||
|
||||
self._stop_background = False
|
||||
self._background_thread = threading.Thread(target=self._background_reading_task)
|
||||
self._background_thread.daemon = True # 设置为守护线程
|
||||
self._background_thread.start()
|
||||
return True
|
||||
|
||||
def stop_background_reading(self):
|
||||
"""
|
||||
停止后台读取任务
|
||||
"""
|
||||
if self._background_thread is None or not self._background_thread.is_alive():
|
||||
print("No background reading is running")
|
||||
return False
|
||||
|
||||
self._stop_background = True
|
||||
self._background_thread.join(timeout=2.0) # 等待线程结束，最多等待2秒
|
||||
self._background_thread = None
|
||||
return True
|
||||
|
||||
def _background_reading_task(self):
|
||||
"""
|
||||
后台读取任务的实现
|
||||
"""
|
||||
while not self._stop_background:
|
||||
try:
|
||||
data = self.read_data_f32()
|
||||
if isinstance(data, np.ndarray):
|
||||
with self._reading_lock:
|
||||
self._last_reading = data
|
||||
else:
|
||||
self.connect()
|
||||
except Exception as e:
|
||||
print(f"Error in background reading: {e}")
|
||||
time.sleep(1.0) # 每秒读取一次
|
||||
|
||||
def get_last_reading(self):
|
||||
"""
|
||||
获取最近一次的读数
|
||||
"""
|
||||
with self._reading_lock:
|
||||
return self._last_reading
|
||||
|
||||
# ------------------------- 以下方法保持不变 -------------------------
|
||||
@staticmethod
|
||||
def generate_crc16_table():
|
||||
"""CRC16 表生成（与原代码相同）"""
|
||||
table = []
|
||||
polynomial = 0xA001
|
||||
for i in range(256):
|
||||
crc = i
|
||||
for _ in range(8):
|
||||
if crc & 0x0001:
|
||||
crc = (crc >> 1) ^ polynomial
|
||||
else:
|
||||
crc >>= 1
|
||||
table.append(crc)
|
||||
return table
|
||||
|
||||
def crc16(self, data: bytes):
|
||||
"""CRC16 计算（与原代码相同）"""
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc = (crc >> 8) ^ self.crc16_table[(crc ^ byte) & 0xFF]
|
||||
return (crc >> 8) & 0xFF, crc & 0xFF
|
||||
|
||||
def __del__(self):
|
||||
"""析构时自动断开连接"""
|
||||
self.disconnect()
|
||||
|
||||
def disable_active_transmission(self) -> int:
|
||||
"""
|
||||
禁用传感器的主动传输模式（TCP 版本）
|
||||
Returns:
|
||||
0 表示成功，-1 表示失败
|
||||
"""
|
||||
try:
|
||||
# 构造禁用命令（原代码中的 FF x11 字节流）
|
||||
disable_cmd = bytes.fromhex('FF FF FF FF FF FF FF FF FF FF FF')
|
||||
print("Disabling active transmission mode...")
|
||||
|
||||
# 检查 TCP 连接是否有效
|
||||
if not self.sock:
|
||||
raise ConnectionError("TCP connection is not established")
|
||||
|
||||
# 发送禁用命令
|
||||
self.sock.sendall(disable_cmd)
|
||||
print("Active transmission disabled successfully")
|
||||
return 0
|
||||
|
||||
except ConnectionError as e:
|
||||
print(f"Connection error: {e}")
|
||||
return -1
|
||||
except socket.timeout:
|
||||
print("Timeout while sending disable command")
|
||||
return -1
|
||||
except Exception as e:
|
||||
print(f"Unexpected error: {e}")
|
||||
return -1
|
||||
|
||||
def read(self):
|
||||
"""
|
||||
TCP Version - Read the sensor's data in passive mode.
|
||||
Returns:
|
||||
np.ndarray(6,) - Six-axis force/torque data [Fx, Fy, Fz, Mx, My, Mz]
|
||||
None if reading fails
|
||||
"""
|
||||
try:
|
||||
# Clear any existing data in the TCP buffer first
|
||||
# self._clear_tcp_buffer()
|
||||
|
||||
# Search for frame header (0x20 0x4E)
|
||||
header_found = False
|
||||
start_time = time.time()
|
||||
timeout = 1.0 # 1 second timeout
|
||||
|
||||
while not header_found and (time.time() - start_time < timeout):
|
||||
# Read one byte at a time looking for header
|
||||
byte1 = self.sock.recv(1)
|
||||
if not byte1:
|
||||
continue # No data available yet
|
||||
|
||||
if byte1 == b'\x20':
|
||||
byte2 = self.sock.recv(1)
|
||||
if byte2 == b'\x4E':
|
||||
header_found = True
|
||||
|
||||
if not header_found:
|
||||
print("Frame header not found within timeout period")
|
||||
return None
|
||||
|
||||
# Now read the remaining 14 bytes of the frame
|
||||
response = bytearray([0x20, 0x4E]) # Start with the header we found
|
||||
remaining_bytes = 14
|
||||
bytes_received = 0
|
||||
start_time = time.time()
|
||||
|
||||
while bytes_received < remaining_bytes and (time.time() - start_time < timeout):
|
||||
chunk = self.sock.recv(remaining_bytes - bytes_received)
|
||||
if chunk:
|
||||
response.extend(chunk)
|
||||
bytes_received += len(chunk)
|
||||
|
||||
if bytes_received < remaining_bytes:
|
||||
print(f"Incomplete frame received. Got {len(response)} bytes, expected 16")
|
||||
return None
|
||||
|
||||
# Verify CRC checksum
|
||||
Hi_check, Lo_check = self.crc16(response[:-2])
|
||||
if response[-1] != Hi_check or response[-2] != Lo_check:
|
||||
print("CRC check failed!")
|
||||
print(f"Received CRC: {response[-2]:02X}{response[-1]:02X}")
|
||||
print(f"Calculated CRC: {Lo_check:02X}{Hi_check:02X}")
|
||||
return None
|
||||
|
||||
# Parse the sensor data
|
||||
sensor_data = self.parse_data_passive(response)
|
||||
return sensor_data
|
||||
|
||||
except socket.timeout:
|
||||
print("Timeout while waiting for sensor data")
|
||||
return None
|
||||
except ConnectionError as e:
|
||||
print(f"TCP connection error: {e}")
|
||||
return None
|
||||
except Exception as e:
|
||||
print(f"Unexpected error in TCP read(): {str(e)}")
|
||||
return None
|
||||
|
||||
def parse_data_passive(self, buffer):
|
||||
values = [
|
||||
int.from_bytes(buffer[i:i+2], byteorder='little', signed=True)
|
||||
for i in range(2, 14, 2)
|
||||
]
|
||||
Fx, Fy, Fz = np.array(values[:3]) / 10.0
|
||||
Mx, My, Mz = np.array(values[3:]) / 1000.0
|
||||
return np.array([Fx, Fy, Fz, Mx, My, Mz])
|
||||
|
||||
|
||||
# 外部
|
||||
def test_sensor_frequency(sensor, mode='active', duration=5.0):
|
||||
"""
|
||||
测试传感器在不同模式下的实际数据获取频率
|
||||
|
||||
参数:
|
||||
sensor: XjcSensor实例
|
||||
mode: 'active'(主动模式) 或 'polling'(查询模式)
|
||||
duration: 测试持续时间(秒)
|
||||
|
||||
返回:
|
||||
dict: 包含测试结果的字典
|
||||
"""
|
||||
# 准备测试环境
|
||||
if mode == 'active':
|
||||
print("\n=== 测试主动传输模式 ===")
|
||||
sensor.enable_active_transmission()
|
||||
read_func = sensor.read
|
||||
else:
|
||||
print("\n=== 测试查询模式 ===")
|
||||
sensor.disable_active_transmission() # 确保不在主动模式
|
||||
time.sleep(0.5)
|
||||
sensor.disable_active_transmission() # 确保不在主动模式
|
||||
time.sleep(0.5)
|
||||
sensor.disable_active_transmission() # 确保不在主动模式
|
||||
time.sleep(0.5)
|
||||
read_func = sensor.read_data_f32
|
||||
|
||||
# 初始化测试变量
|
||||
timestamps = []
|
||||
data_count = 0
|
||||
start_time = time.perf_counter()
|
||||
end_time = start_time + duration
|
||||
|
||||
print(f"开始测试，持续时间 {duration} 秒...")
|
||||
|
||||
# 主测试循环
|
||||
while time.perf_counter() < end_time:
|
||||
data = read_func()
|
||||
print(data)
|
||||
if data is not None:
|
||||
timestamps.append(time.perf_counter())
|
||||
data_count += 1
|
||||
# print(timestamps)
|
||||
# 计算统计结果
|
||||
if data_count < 2:
|
||||
print("警告: 采集到的数据点不足")
|
||||
return None
|
||||
|
||||
intervals = np.diff(timestamps)
|
||||
save_intervals_to_csv(timestamps)
|
||||
avg_interval = np.mean(intervals)
|
||||
min_interval = np.min(intervals)
|
||||
max_interval = np.max(intervals)
|
||||
std_interval = np.std(intervals)
|
||||
avg_freq = 1.0 / avg_interval
|
||||
|
||||
# 打印结果
|
||||
print("\n测试结果:")
|
||||
print(f"总数据点数: {data_count}")
|
||||
print(f"平均频率: {avg_freq:.2f} Hz")
|
||||
print(f"最小间隔: {min_interval*1000:.3f} ms")
|
||||
print(f"最大间隔: {max_interval*1000:.3f} ms")
|
||||
print(f"间隔标准差: {std_interval*1000:.3f} ms")
|
||||
|
||||
# 返回结果字典
|
||||
return {
|
||||
'mode': mode,
|
||||
'duration': duration,
|
||||
'data_count': data_count,
|
||||
'avg_freq': avg_freq,
|
||||
'min_interval': min_interval,
|
||||
'max_interval': max_interval,
|
||||
'std_interval': std_interval,
|
||||
'timestamps': timestamps,
|
||||
'intervals': intervals
|
||||
}
|
||||
|
||||
def plot_test_results(active_results, polling_results):
|
||||
"""绘制两种模式的测试结果对比图"""
|
||||
plt.figure(figsize=(12, 8))
|
||||
|
||||
# 间隔时间分布图
|
||||
plt.subplot(2, 1, 1)
|
||||
plt.hist(active_results['intervals']*1000, bins=50, alpha=0.7, label='主动模式')
|
||||
plt.hist(polling_results['intervals']*1000, bins=50, alpha=0.7, label='查询模式')
|
||||
plt.xlabel('间隔时间 (ms)')
|
||||
plt.ylabel('出现次数')
|
||||
plt.title('数据间隔时间分布')
|
||||
plt.legend()
|
||||
plt.grid(True)
|
||||
|
||||
# 间隔时间序列图
|
||||
plt.subplot(2, 1, 2)
|
||||
plt.plot(np.arange(len(active_results['intervals'])),
|
||||
active_results['intervals']*1000, 'b.', label='主动模式')
|
||||
plt.plot(np.arange(len(polling_results['intervals'])),
|
||||
polling_results['intervals']*1000, 'r.', label='查询模式')
|
||||
plt.xlabel('数据点序号')
|
||||
plt.ylabel('间隔时间 (ms)')
|
||||
plt.title('数据间隔时间序列')
|
||||
plt.legend()
|
||||
plt.grid(True)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
def compare_modes(sensor, duration=5.0):
|
||||
"""比较两种工作模式的性能"""
|
||||
# 测试主动模式
|
||||
active_results = test_sensor_frequency(sensor, 'active', duration)
|
||||
time.sleep(1) # 模式切换间隔
|
||||
|
||||
# 测试查询模式
|
||||
polling_results = test_sensor_frequency(sensor, 'polling', duration)
|
||||
time.sleep(1)
|
||||
|
||||
# 打印对比报告
|
||||
print("\n=== 模式对比报告 ===")
|
||||
print(f"{'指标':<15} {'主动模式':>15} {'查询模式':>15}")
|
||||
print(f"{'平均频率(Hz)':<15} {active_results['avg_freq']:>15.2f} {polling_results['avg_freq']:>15.2f}")
|
||||
print(f"{'最小间隔(ms)':<15} {active_results['min_interval']*1000:>15.3f} {polling_results['min_interval']*1000:>15.3f}")
|
||||
print(f"{'最大间隔(ms)':<15} {active_results['max_interval']*1000:>15.3f} {polling_results['max_interval']*1000:>15.3f}")
|
||||
print(f"{'间隔标准差(ms)':<15} {active_results['std_interval']*1000:>15.3f} {polling_results['std_interval']*1000:>15.3f}")
|
||||
|
||||
# 绘制对比图表
|
||||
plot_test_results(active_results, polling_results)
|
||||
|
||||
return active_results, polling_results
|
||||
|
||||
def save_intervals_to_csv(timestamps, filename="sensor_intervals.csv"):
|
||||
"""
|
||||
Save timestamp intervals to CSV file with metadata
|
||||
|
||||
Args:
|
||||
timestamps: List of timestamp values (from time.perf_counter())
|
||||
filename: Output CSV filename
|
||||
"""
|
||||
if len(timestamps) < 2:
|
||||
print("Not enough timestamps to calculate intervals")
|
||||
return
|
||||
|
||||
# Calculate intervals in milliseconds
|
||||
intervals_ms = np.diff(timestamps) * 1000
|
||||
|
||||
# Prepare data rows
|
||||
rows = []
|
||||
for i, interval in enumerate(intervals_ms):
|
||||
rows.append({
|
||||
"reading_number": i+1,
|
||||
"interval_ms": f"{interval:.4f}",
|
||||
"timestamp": timestamps[i],
|
||||
"expected_interval_ms": (1000/250) if i==0 else "", # For 250Hz
|
||||
"deviation_ms": f"{interval - (1000/250):.4f}" if i>0 else ""
|
||||
})
|
||||
|
||||
# CSV header
|
||||
fieldnames = ["reading_number", "interval_ms", "timestamp",
|
||||
"expected_interval_ms", "deviation_ms"]
|
||||
|
||||
# Write to file
|
||||
with open(filename, 'w', newline='') as csvfile:
|
||||
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
|
||||
writer.writeheader()
|
||||
writer.writerows(rows)
|
||||
|
||||
print(f"Saved {len(intervals_ms)} intervals to {filename}")
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 替换为你的传感器 IP 和端口
|
||||
sensor = XjcSensor("192.168.5.1", 60000)
|
||||
|
||||
# 示例操作
|
||||
|
||||
sensor.disable_active_transmission()
|
||||
time.sleep(0.5)
|
||||
sensor.disable_active_transmission()
|
||||
time.sleep(0.5)
|
||||
sensor.disable_active_transmission()
|
||||
time.sleep(0.5)
|
||||
sensor.set_zero()
|
||||
|
||||
# sensor.set_zero()
|
||||
# sensor.disable_active_transmission()
|
||||
|
||||
# while True:
|
||||
# data = sensor.read_data_f32()
|
||||
# if data is not None:
|
||||
# print(f"Force data: {data}")
|
||||
# sensor.enable_active_transmission()
|
||||
# while True:
|
||||
# sensor_data = sensor.read()
|
||||
# if sensor_data is None:
|
||||
# print('failed to get force sensor data!')
|
||||
# print(sensor_data)
|
||||
test_sensor_frequency(sensor)
|
||||
# compare_modes(sensor)
|
||||
Loading…
x
Reference in New Issue
Block a user