暂存修改
This commit is contained in:
Ziwei.He
parent
04364f7a95
commit
52c84bb914
@ -1,24 +1,24 @@
|
||||
from functools import wraps
|
||||
import sys
|
||||
import numpy as np
|
||||
import threading
|
||||
import time
|
||||
from typing import List, Literal, Union
|
||||
import sys
|
||||
import numpy as np
|
||||
import threading
|
||||
import time
|
||||
from typing import List, Literal, Union
|
||||
|
||||
from scipy.interpolate import interp1d
|
||||
from scipy.interpolate import interp1d
|
||||
|
||||
import requests
|
||||
import os
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import ctypes
|
||||
libc = ctypes.CDLL("libc.so.6")
|
||||
import atexit
|
||||
import copy
|
||||
import requests
|
||||
import os
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import ctypes
|
||||
libc = ctypes.CDLL("libc.so.6")
|
||||
import atexit
|
||||
import copy
|
||||
|
||||
from scipy.spatial.transform import Slerp
|
||||
from scipy.spatial.transform import Slerp
|
||||
|
||||
|
||||
try:
|
||||
try:
|
||||
# 导入算法
|
||||
from .algorithms.arm_state import ArmState
|
||||
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.serial_tools import find_serial_by_location
|
||||
from .tools.Trajectory import TrajectoryInterpolator
|
||||
except:
|
||||
except:
|
||||
#导入算法
|
||||
from algorithms.arm_state import ArmState
|
||||
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.Trajectory import TrajectoryInterpolator
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.animation import FuncAnimation
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.animation import FuncAnimation
|
||||
|
||||
current_file_path = os.path.abspath(__file__)
|
||||
MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
|
||||
print(MassageRobot_Dobot_Path)
|
||||
# 添加目标文件夹到系统路径
|
||||
sys.path.append(MassageRobot_Dobot_Path)
|
||||
from VortXDB.client import VTXClient
|
||||
current_file_path = os.path.abspath(__file__)
|
||||
MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
|
||||
print(MassageRobot_Dobot_Path)
|
||||
# 添加目标文件夹到系统路径
|
||||
sys.path.append(MassageRobot_Dobot_Path)
|
||||
from VortXDB.client import VTXClient
|
||||
|
||||
"""
|
||||
在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。
|
||||
"""
|
||||
在 Python 中，atexit.register 用于注册程序退出时要执行的函数。atexit 模块提供了一个简单的接口，用于在程序正常终止时执行清理操作。
|
||||
当你注册多个函数时，它们的执行顺序遵循先进后出的顺序（LIFO：Last In, First Out）。也就是说，最后注册的函数会最先执行。
|
||||
"""
|
||||
"""
|
||||
|
||||
def track_function(function_name,log = False):
|
||||
def track_function(function_name,log = False):
|
||||
def before(self, *args, **kwargs):
|
||||
self.current_function = function_name
|
||||
if log:
|
||||
@ -97,8 +97,8 @@ from functools import wraps
|
||||
self.logger.log_info(f"Exiting function: {function_name},code: {result}")
|
||||
return custom_decorator(before=before, after=after)
|
||||
|
||||
@apply_decorators
|
||||
class MassageRobot:
|
||||
@apply_decorators
|
||||
class MassageRobot:
|
||||
def __init__(self,arm_config_path,is_log=False):
|
||||
self.logger = CustomLogger()
|
||||
if is_log:
|
||||
@ -113,9 +113,14 @@ from functools import wraps
|
||||
self.arm_state = ArmState()
|
||||
self.arm_config = read_yaml(arm_config_path)
|
||||
# arm 实例化时机械臂类内部进行通讯连接
|
||||
# 初始化坐标系 TOOL=0 BASE=1
|
||||
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
|
||||
self.arm.start()
|
||||
<<<<<<< HEAD
|
||||
self.arm.chooseRightFrame()
|
||||
=======
|
||||
self.arm.init()
|
||||
>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
|
||||
|
||||
self.thermotherapy = None
|
||||
self.shockwave = None
|
||||
@ -130,18 +135,30 @@ from functools import wraps
|
||||
# 控制器,初始为导纳控制
|
||||
self.controller_manager = ControllerManager(self.arm_state)
|
||||
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(PositionController,'position',self.arm_config['controller'][2])
|
||||
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(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
|
||||
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(HybridController,'hybrid',self.arm_config['controller'][1])
|
||||
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(HybridPidController,'hybridPid',self.arm_config['controller'][4])
|
||||
# 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(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
|
||||
|
||||
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'])
|
||||
@ -540,8 +557,9 @@ from functools import wraps
|
||||
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 = (
|
||||
f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
|
||||
f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
|
||||
@ -1567,36 +1585,36 @@ from functools import wraps
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
waypoints = [
|
||||
{"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
|
||||
"acceleration": [0, 0, 0],
|
||||
"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],
|
||||
"acceleration": [0, 0, 0],
|
||||
"orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
] ## 单位 m deg
|
||||
myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
ts = np.linspace(0,5,100)
|
||||
if __name__ == "__main__":
|
||||
# waypoints = [
|
||||
# {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
|
||||
# "acceleration": [0, 0, 0],
|
||||
# "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],
|
||||
# "acceleration": [0, 0, 0],
|
||||
# "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
# ] ## 单位 m deg
|
||||
# myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
# ts = np.linspace(0,5,100)
|
||||
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
|
||||
xr_list, vr_list, ar_list = [], [], []
|
||||
for t in ts:
|
||||
xr, vr, ar = myInterpolate.interpolate(t)
|
||||
xr_list.append(xr)
|
||||
vr_list.append(vr)
|
||||
ar_list.append(ar)
|
||||
xr_array = np.array(xr_list)
|
||||
vr_array = np.array(vr_list)
|
||||
ar_array = np.array(ar_list)
|
||||
# xr_list, vr_list, ar_list = [], [], []
|
||||
# for t in ts:
|
||||
# xr, vr, ar = myInterpolate.interpolate(t)
|
||||
# xr_list.append(xr)
|
||||
# vr_list.append(vr)
|
||||
# ar_list.append(ar)
|
||||
# xr_array = np.array(xr_list)
|
||||
# vr_array = np.array(vr_list)
|
||||
# ar_array = np.array(ar_list)
|
||||
|
||||
robot.xr = xr_array
|
||||
robot.vr = vr_array
|
||||
robot.ar = ar_array
|
||||
robot.ts = ts
|
||||
robot.dt = ts[1] - ts[0]
|
||||
# robot.xr = xr_array
|
||||
# robot.vr = vr_array
|
||||
# robot.ar = ar_array
|
||||
# robot.ts = ts
|
||||
# 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.force_sensor.disable_active_transmission()
|
||||
@ -1628,3 +1646,4 @@ from functools import wraps
|
||||
print("发生异常：", e)
|
||||
|
||||
|
||||
# robot.arm.disableRobot()
|
||||
Loading…
x
Reference in New Issue
Block a user