DevelopmentTeam/MassageRobot_Dobot
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

暂存修改

Browse Source
This commit is contained in:
Ziwei.He 2025-05-28 21:18:29 +08:00
commit 04364f7a95
1 changed files with 1628 additions and 1620 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

292
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -1,24 +1,24 @@
from functools import wraps from functools import wraps
import sys import sys
import numpy as np import numpy as np
import threading import threading
import time import time
from typing import List, Literal, Union from typing import List, Literal, Union
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
import requests import requests
import os import os
from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Rotation as R
import ctypes import ctypes
libc = ctypes.CDLL("libc.so.6") libc = ctypes.CDLL("libc.so.6")
import atexit import atexit
import copy import copy
from scipy.spatial.transform import Slerp from scipy.spatial.transform import Slerp
try: try:
# 导入算法 # 导入算法
from .algorithms.arm_state import ArmState from .algorithms.arm_state import ArmState
from .algorithms.controller_manager import ControllerManager from .algorithms.controller_manager import ControllerManager
@ -44,7 +44,7 @@ try:
from .tools.decorator_tools import custom_decorator,apply_decorators from .tools.decorator_tools import custom_decorator,apply_decorators
from .tools.serial_tools import find_serial_by_location from .tools.serial_tools import find_serial_by_location
from .tools.Trajectory import TrajectoryInterpolator from .tools.Trajectory import TrajectoryInterpolator
except: except:
#导入算法 #导入算法
from algorithms.arm_state import ArmState from algorithms.arm_state import ArmState
from algorithms.controller_manager import ControllerManager from algorithms.controller_manager import ControllerManager
@ -71,22 +71,22 @@ except:
from tools.serial_tools import find_serial_by_location from tools.serial_tools import find_serial_by_location
from tools.Trajectory import TrajectoryInterpolator from tools.Trajectory import TrajectoryInterpolator
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation from matplotlib.animation import FuncAnimation
current_file_path = os.path.abspath(__file__) current_file_path = os.path.abspath(__file__)
MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path))) MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
print(MassageRobot_Dobot_Path) print(MassageRobot_Dobot_Path)
# 添加目标文件夹到系统路径 # 添加目标文件夹到系统路径
sys.path.append(MassageRobot_Dobot_Path) sys.path.append(MassageRobot_Dobot_Path)
from VortXDB.client import VTXClient from VortXDB.client import VTXClient
""" """
在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。 在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。
当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。 当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。
""" """
def track_function(function_name,log = False): def track_function(function_name,log = False):
def before(self, *args, **kwargs): def before(self, *args, **kwargs):
self.current_function = function_name self.current_function = function_name
if log: if log:
@ -97,8 +97,8 @@ def track_function(function_name,log = False):
self.logger.log_info(f"Exiting function: {function_name},code: {result}") self.logger.log_info(f"Exiting function: {function_name},code: {result}")
return custom_decorator(before=before, after=after) return custom_decorator(before=before, after=after)
@apply_decorators @apply_decorators
class MassageRobot: class MassageRobot:
def __init__(self,arm_config_path,is_log=False): def __init__(self,arm_config_path,is_log=False):
self.logger = CustomLogger() self.logger = CustomLogger()
if is_log: if is_log:
@ -108,13 +108,14 @@ class MassageRobot:
self.vtxdb = VTXClient() self.vtxdb = VTXClient()
# 当前执行的函数
self.current_function = None
self.arm_state = ArmState() self.arm_state = ArmState()
self.arm_config = read_yaml(arm_config_path) self.arm_config = read_yaml(arm_config_path)
# arm 实例化时机械臂类内部进行通讯连接 # arm 实例化时机械臂类内部进行通讯连接
# 初始化坐标系 TOOL=0 BASE=1
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip']) self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
self.arm.start() self.arm.start()
self.arm.chooseRightFrame() self.arm.init()
self.thermotherapy = None self.thermotherapy = None
self.shockwave = None self.shockwave = None
@ -129,35 +130,18 @@ class MassageRobot:
# 控制器,初始为导纳控制 # 控制器,初始为导纳控制
self.controller_manager = ControllerManager(self.arm_state) self.controller_manager = ControllerManager(self.arm_state)
self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0]) self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
# self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1]) self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1]) self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
# self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3]) self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
# self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4]) self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
# self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5]) self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
# self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6]) self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
# self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7]) self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
self.controller_manager.switch_controller('admittance') 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))]
self.playload_dict = {}
for file in head_config_files:
file_address = massage_head_dir + '/' + file
play_load = read_yaml(file_address)
self.playload_dict[play_load['name']] = play_load
self.current_head = 'none'
# 读取数据
self.gravity_base = None
self.force_zero = None
self.torque_zero = None
self.tool_position = None
self.mass_center_position = None
self.s_tf_matrix_t = None
arm_orientation_set0 = np.array([-180,-30,0])
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
# 频率数据赋值 # 频率数据赋值
self.control_rate = Rate(self.arm_config['control_rate']) self.control_rate = Rate(self.arm_config['control_rate'])
@ -179,15 +163,30 @@ class MassageRobot:
# 按摩调整 # 按摩调整
self.adjust_wrench_envent = threading.Event() self.adjust_wrench_envent = threading.Event()
self.adjust_wrench_envent.clear() # 调整初始化为False self.adjust_wrench_envent.clear() # 调整初始化为False
self.is_execute = False
self.pos_increment = np.zeros(3,dtype=np.float64) self.pos_increment = np.zeros(3,dtype=np.float64)
self.adjust_wrench = np.zeros(6,dtype=np.float64) self.adjust_wrench = np.zeros(6,dtype=np.float64)
self.skip_pos = np.zeros(6,dtype=np.float64)
# 按摩头参数暂时使用本地数据
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))]
self.playload_dict = {}
for file in head_config_files:
file_address = massage_head_dir + '/' + file
play_load = read_yaml(file_address)
self.playload_dict[play_load['name']] = play_load
# self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
# print(self.playload_dict)
self.current_head = 'none'
self.is_waitting = False self.is_waitting = False
self.last_print_time = 0 self.last_print_time = 0
self.last_record_time = 0 self.last_record_time = 0
self.last_command_time = 0 self.last_command_time = 0
self.move_to_point_count = 0 self.move_to_point_count = 0
self.width_default = 5
# 卡尔曼滤波相关初始化 # 卡尔曼滤波相关初始化
self.x_base = np.zeros(6) self.x_base = np.zeros(6)
self.P_base = np.eye(6) self.P_base = np.eye(6)
@ -205,7 +204,16 @@ class MassageRobot:
# 传感器故障计数器 # 传感器故障计数器
self.sensor_fail_count = 0 self.sensor_fail_count = 0
# 机械臂初始化,适配中间层 # 机械臂初始化,适配中间层
self.arm.init() # 读取数据
self.gravity_base = None
self.force_zero = None
self.torque_zero = None
self.tool_position = None
self.mass_center_position = None
self.s_tf_matrix_t = None
arm_orientation_set0 = np.array([-180,0,-90])
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
# REF TRAJ # REF TRAJ
self.xr = [] self.xr = []
@ -217,6 +225,36 @@ class MassageRobot:
self.last_time = -1 self.last_time = -1
self.cur_time = -1 self.cur_time = -1
# 预测步骤
def kalman_predict(self,x, P, Q):
# 预测状态（这里假设状态不变）
x_predict = x
# 预测误差协方差
P_predict = P + Q
return x_predict, P_predict
# 更新步骤
def kalman_update(self,x_predict, P_predict, z, R):
# 卡尔曼增益
# K = P_predict @ np.linalg.inv(P_predict + R)
S = P_predict + R
s = np.diag(S) # shape (6,)
p_diag = np.diag(P_predict)
K_diag = np.zeros_like(s)
nonzero_mask = s != 0
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
K = np.diag(K_diag)
# 更新状态
x_update = x_predict + K @ (z - x_predict)
# 更新误差协方差
P_update = (np.eye(len(K)) - K) @ P_predict
return x_update, P_update
def init_hardwares(self,ready_pose):
self.ready_pose = np.array(ready_pose)
self.switch_payload(self.current_head)
print(self.arm.get_arm_position())
time.sleep(0.5)
def sensor_set_zero(self): def sensor_set_zero(self):
self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position() self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position()
@ -325,30 +363,7 @@ class MassageRobot:
return -1 return -1
return 0 return 0
# 预测步骤
def kalman_predict(self,x, P, Q):
# 预测状态（这里假设状态不变）
x_predict = x
# 预测误差协方差
P_predict = P + Q
return x_predict, P_predict
# 更新步骤
def kalman_update(self,x_predict, P_predict, z, R):
# 卡尔曼增益
# K = P_predict @ np.linalg.inv(P_predict + R)
S = P_predict + R
s = np.diag(S) # shape (6,)
p_diag = np.diag(P_predict)
K_diag = np.zeros_like(s)
nonzero_mask = s != 0
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
K = np.diag(K_diag)
# 更新状态
x_update = x_predict + K @ (z - x_predict)
# 更新误差协方差
P_update = (np.eye(len(K)) - K) @ P_predict
return x_update, P_update
def update_wrench(self): def update_wrench(self):
# 当前的机械臂到末端转换 (实时) # 当前的机械臂到末端转换 (实时)
@ -386,6 +401,29 @@ class MassageRobot:
self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
return 0 return 0
def switch_payload(self,name):
if name in self.playload_dict:
self.stop()
self.current_head = name
compensation_config = self.playload_dict[self.current_head]
# 读取数据
self.gravity_base = np.array(compensation_config['gravity_base'])
self.force_zero = np.array(compensation_config['force_zero'])
self.torque_zero = np.array(compensation_config['torque_zero'])
self.tool_position = np.array(compensation_config['tcp_offset'])
self.mass_center_position = np.array(compensation_config['mass_center_position'])
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
tcp_offset = self.playload_dict[name]["tcp_offset"]
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
print("tcp_offset_str",tcp_offset_str)
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
self.arm.chooseEndEffector(i=9)
print(self.arm.get_arm_position())
self.logger.log_info(f"切换到{name}按摩头")
# ####################### 位姿伺服 ########################## # ####################### 位姿伺服 ##########################
def arm_measure_loop(self): def arm_measure_loop(self):
@ -502,9 +540,8 @@ class MassageRobot:
command_pose[:3], command_pose[:3],
command_pose[3:] command_pose[3:]
) )
# pose_processed = command_pose # print(f"pose_processed:{pose_processed}")
print(f"pose_processed:{pose_processed}")
print(self.arm.feedbackData.robotMode)
tcp_command = ( tcp_command = (
f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f}," f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
f"{pose_processed[2]:.3f},{pose_processed[3]:.3f}," f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
@ -538,7 +575,7 @@ class MassageRobot:
# 线程 # 线程
self.exit_event.clear() self.exit_event.clear()
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop) self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
self.arm_control_thread = threading.Thread(target=self.arm_command_loop) self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
# 线程启动 # 线程启动
self.arm_measure_thread.start() ## 测量线程 self.arm_measure_thread.start() ## 测量线程
position,quat_rot = self.arm.get_arm_position() position,quat_rot = self.arm.get_arm_position()
@ -587,34 +624,7 @@ class MassageRobot:
self.arm.stop_motion() self.arm.stop_motion()
self.logger.log_info("MassageRobot停止") self.logger.log_info("MassageRobot停止")
def init_hardwares(self,ready_pose):
self.ready_pose = np.array(ready_pose)
self.switch_payload(self.current_head)
print(self.arm.get_arm_position())
time.sleep(0.5)
def switch_payload(self,name):
if name in self.playload_dict:
self.stop()
self.current_head = name
compensation_config = self.playload_dict[self.current_head]
# 读取数据
self.gravity_base = np.array(compensation_config['gravity_base'])
self.force_zero = np.array(compensation_config['force_zero'])
self.torque_zero = np.array(compensation_config['torque_zero'])
self.tool_position = np.array(compensation_config['tcp_offset'])
self.mass_center_position = np.array(compensation_config['mass_center_position'])
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
tcp_offset = self.playload_dict[name]["tcp_offset"]
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
print("tcp_offset_str",tcp_offset_str)
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
self.arm.chooseEndEffector(i=9)
print(self.arm.get_arm_position())
self.logger.log_info(f"切换到{name}按摩头")
self.controller_manager.switch_controller('position') self.controller_manager.switch_controller('position')
else: else:
self.logger.log_error(f"未找到{name}按摩头") self.logger.log_error(f"未找到{name}按摩头")
@ -1557,36 +1567,36 @@ class MassageRobot:
if __name__ == "__main__": if __name__ == "__main__":
# waypoints = [ waypoints = [
# {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
# "acceleration": [0, 0, 0], "acceleration": [0, 0, 0],
# "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()}, "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
# {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0], {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
# "acceleration": [0, 0, 0], "acceleration": [0, 0, 0],
# "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
# ] ## 单位 m deg ] ## 单位 m deg
# myInterpolate = TrajectoryInterpolator(waypoints) myInterpolate = TrajectoryInterpolator(waypoints)
# ts = np.linspace(0,5,100) ts = np.linspace(0,5,100)
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml") robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
# xr_list, vr_list, ar_list = [], [], [] xr_list, vr_list, ar_list = [], [], []
# for t in ts: for t in ts:
# xr, vr, ar = myInterpolate.interpolate(t) xr, vr, ar = myInterpolate.interpolate(t)
# xr_list.append(xr) xr_list.append(xr)
# vr_list.append(vr) vr_list.append(vr)
# ar_list.append(ar) ar_list.append(ar)
# xr_array = np.array(xr_list) xr_array = np.array(xr_list)
# vr_array = np.array(vr_list) vr_array = np.array(vr_list)
# ar_array = np.array(ar_list) ar_array = np.array(ar_list)
# robot.xr = xr_array robot.xr = xr_array
# robot.vr = vr_array robot.vr = vr_array
# robot.ar = ar_array robot.ar = ar_array
# robot.ts = ts robot.ts = ts
# robot.dt = ts[1] - ts[0] robot.dt = ts[1] - ts[0]
ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
robot.current_head = 'finger_head' robot.current_head = 'finger_head'
robot.force_sensor.disable_active_transmission() robot.force_sensor.disable_active_transmission()
@ -1611,12 +1621,10 @@ if __name__ == "__main__":
try: try:
robot_thread = threading.Thread(target=robot.start) robot_thread = threading.Thread(target=robot.start)
robot_thread.start() robot_thread.start()
except KeyboardInterrupt: except KeyboardInterrupt:
print("用户中断操作。") print("用户中断操作。")
except Exception as e: except Exception as e:
print("Exception occurred at line:", e.__traceback__.tb_lineno) print("Exception occurred at line:", e.__traceback__.tb_lineno)
print("发生异常：", e) print("发生异常：", e)
robot_thread.join() # 等待机械臂线程结束
# robot.arm.disableRobot()