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:19:18 +08:00
parent 04364f7a95
commit 52c84bb914
1 changed files with 1647 additions and 1628 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
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 @@ from functools import wraps
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 @@ from functools import wraps
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 @@ from functools import wraps
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:
@ -113,9 +113,14 @@ from functools import wraps
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()
<<<<<<< HEAD
self.arm.chooseRightFrame()
=======
self.arm.init() self.arm.init()
>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
self.thermotherapy = None self.thermotherapy = None
self.shockwave = None self.shockwave = None
@ -130,18 +135,30 @@ from functools import wraps
# 控制器,初始为导纳控制 # 控制器,初始为导纳控制
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'][2]) self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
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')
<<<<<<< HEAD
# 读取数据
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()
=======
>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
# 频率数据赋值 # 频率数据赋值
self.control_rate = Rate(self.arm_config['control_rate']) self.control_rate = Rate(self.arm_config['control_rate'])
@ -540,8 +557,9 @@ from functools import wraps
command_pose[:3], command_pose[:3],
command_pose[3:] command_pose[3:]
) )
# print(f"pose_processed:{pose_processed}") # pose_processed = command_pose
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},"
@ -1567,36 +1585,36 @@ from functools import wraps
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 = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0] ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
robot.current_head = 'finger_head' robot.current_head = 'finger_head'
robot.force_sensor.disable_active_transmission() robot.force_sensor.disable_active_transmission()
@ -1628,3 +1646,4 @@ from functools import wraps
print("发生异常：", e) print("发生异常：", e)
# robot.arm.disableRobot()