diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index f656d81..11b9fbf 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -111,8 +111,10 @@ class MassageRobot: 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() + self.arm.chooseRightFrame() self.thermotherapy = None self.shockwave = None @@ -127,13 +129,13 @@ class MassageRobot: # 控制器,初始为导纳控制 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') # 按摩头参数暂时使用本地数据 @@ -154,7 +156,7 @@ class MassageRobot: self.tool_position = None self.mass_center_position = None self.s_tf_matrix_t = None - arm_orientation_set0 = np.array([-180,0,-90]) + arm_orientation_set0 = np.array([-180,-30,0]) self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix() # 频率数据赋值 @@ -499,9 +501,10 @@ class MassageRobot: status, pose_processed = self.arm.process_command( 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}," @@ -1555,35 +1558,35 @@ class MassageRobot: 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) + # 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() diff --git a/Massage/MassageControl/algorithms/__pycache__/admithybrid_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admithybrid_controller.cpython-39.pyc new file mode 100644 index 0000000..1859af2 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/admithybrid_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/admittanceZ_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admittanceZ_controller.cpython-39.pyc new file mode 100644 index 0000000..8ed67e8 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/admittanceZ_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc index 5f8f728..965de51 100644 Binary files a/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc and b/Massage/MassageControl/algorithms/__pycache__/arm_state.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/force_planner.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/force_planner.cpython-39.pyc new file mode 100644 index 0000000..62f3046 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/force_planner.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/hybridAdmit_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/hybridAdmit_controller.cpython-39.pyc new file mode 100644 index 0000000..9ed49c7 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/hybridAdmit_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/hybridPid_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/hybridPid_controller.cpython-39.pyc new file mode 100644 index 0000000..e4dd152 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/hybridPid_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/hybrid_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/hybrid_controller.cpython-39.pyc new file mode 100644 index 0000000..ccb7c64 Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/hybrid_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/interpolation.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/interpolation.cpython-39.pyc new file mode 100644 index 0000000..9f07dfb Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/interpolation.cpython-39.pyc differ diff --git a/Massage/MassageControl/algorithms/__pycache__/positionerSensor_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/positionerSensor_controller.cpython-39.pyc new file mode 100644 index 0000000..95f876b Binary files /dev/null and b/Massage/MassageControl/algorithms/__pycache__/positionerSensor_controller.cpython-39.pyc differ diff --git a/Massage/MassageControl/config/admittance.yaml b/Massage/MassageControl/config/admittance.yaml index e04f831..a773188 100644 --- a/Massage/MassageControl/config/admittance.yaml +++ b/Massage/MassageControl/config/admittance.yaml @@ -8,10 +8,10 @@ mass_rot: [0.11, 0.11, 0.11] # P # stiff_tran: [300, 300, 300] # stiff_rot: [7, 7, 7] -# stiff_tran: [270, 270, 270] -# stiff_rot: [11, 11, 11] -stiff_tran: [0, 0, 0] +stiff_tran: [270, 270, 270] stiff_rot: [11, 11, 11] +# stiff_tran: [0, 0, 0] +# stiff_rot: [11, 11, 11] # stiff_tran: [100, 100, 100] # stiff_rot: [1, 1, 1] # D diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc index d51dd00..36ee0f7 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc and b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc index 88e09f4..c53eae5 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc and b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc index 304bf5a..daa2ed3 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc and b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index 236ef1f..b4f3c9e 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -1,3 +1,10 @@ +import os +import sys +current_file_path = os.path.abspath(__file__) +dobot_nova5_path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path))) +print(dobot_nova5_path) +# 添加目标文件夹到系统路径 +sys.path.append(dobot_nova5_path) try: from dobot_api import DobotApiFeedBack,DobotApiDashboard except: @@ -6,6 +13,7 @@ import threading from time import sleep,time import re from tools.log import CustomLogger +# from tools.log import copy import numpy as np from scipy.spatial.transform import Rotation as R @@ -61,7 +69,7 @@ class dobot_nova5: self.ToolValue = -1 ''' self.feedbackData = feedbackItem() # 自定义反馈数据结构体 - self.logger = CustomLogger('Dobot_nova5',True) + self.logger = CustomLogger('Dobot_nova5') def parseResultId(self,valueRecv): @@ -99,19 +107,24 @@ class dobot_nova5: def start(self): self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort) self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort) - self.dashboard.EmergencyStop(mode=0) # 松开急停 - self.clearError() # 清除报警 - if self.parseResultId(self.dashboard.EnableRobot())[0] != 0: - print("使能失败:检查29999端口是否被占用") - return - print("机械臂使能成功") - + # 状态反馈线程 feedback_thread = threading.Thread( target=self.GetFeedback # 机器状态反馈线程 ) feedback_thread.daemon = True feedback_thread.start() + self.dashboard.RequestControl() + self.dashboard.EmergencyStop(mode=0) # 松开急停 + self.clearError() # 清除报警 + # 关闭碰撞检测 + self.dashboard.SetCollisionLevel(level=1) + if self.parseResultId(self.dashboard.EnableRobot())[0] != 0: + print("使能失败:检查29999端口是否被占用") + return + print("机械臂使能成功") + + def RunPoint_P_inPose(self,poses_list): ''' @@ -723,7 +736,7 @@ class dobot_nova5: 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] + self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449] self.filter_matirx = np.zeros((1,7)) # 位姿伺服用 self.filter_matrix = np.zeros((1,6)) # 角度伺服用 sleep(1) @@ -843,20 +856,39 @@ class dobot_nova5: return 0, result return -1, None + + def chooseRightFrame(self): + self.chooseBaseFrame(i=1) + # self.chooseEndEffector(i=0) + return if __name__ == "__main__": # sleep(5) dobot = dobot_nova5("192.168.5.1") dobot.start() - posJ = [0,30,-120,0,90,0] - dobot.RunPoint_P_inJoint(posJ) - sleep(1) - dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") - dobot.chooseEndEffector(i=9) - print("Arm pose:",dobot.getPose()) + # posJ_home = [-45,87,-147,0,90,-90] + posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] + dobot.RunPoint_P_inJoint(posJ_ready) + + # sleep(1) + dobot.chooseRightFrame() + # print("Arm pose1:",dobot.getPose()) + # print("Arm pose:",dobot.getPose(user=1,tool=0)) + dobot.setEndEffector(i=8,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") + dobot.chooseEndEffector(i=8) + print("Arm pose2:",dobot.getPose(user=1,tool=8)) + + print(dobot.get_arm_position()) # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) - + cur_pose = dobot.getPose(user=1,tool=8) + cur_pose[0] += 30 + target_pose = cur_pose + print("target",target_pose) + dobot.RunPoint_P_inPose(target_pose) + dobot.ServoPose(dobot.getPose(user=1,tool=8)) + print("new ready joint",dobot.feedbackData.QActual) # dobot.start_drag() + # dobot.stop_drag() dobot.disableRobot() \ No newline at end of file diff --git a/Massage/MassageControl/hardware/force_sensor.py b/Massage/MassageControl/hardware/force_sensor.py index b62ca4c..cc9eda0 100644 --- a/Massage/MassageControl/hardware/force_sensor.py +++ b/Massage/MassageControl/hardware/force_sensor.py @@ -16,7 +16,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro class XjcSensor: def __init__(self, port=None, baudrate=115200, rate=250): - self.port = port + self.port = '/dev/ttyUSB0' self.baudrate = baudrate self.rate = rate diff --git a/Massage/MassageControl/tools/__pycache__/decorator_tools.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/decorator_tools.cpython-39.pyc new file mode 100644 index 0000000..fec5acd Binary files /dev/null and b/Massage/MassageControl/tools/__pycache__/decorator_tools.cpython-39.pyc differ diff --git a/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc index 990b67d..762ad61 100644 Binary files a/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc and b/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc differ diff --git a/Massage/tools/__pycache__/log.cpython-39.pyc b/Massage/tools/__pycache__/log.cpython-39.pyc new file mode 100644 index 0000000..7b504d1 Binary files /dev/null and b/Massage/tools/__pycache__/log.cpython-39.pyc differ diff --git a/VortXDB/__pycache__/client.cpython-39.pyc b/VortXDB/__pycache__/client.cpython-39.pyc new file mode 100644 index 0000000..4d4406c Binary files /dev/null and b/VortXDB/__pycache__/client.cpython-39.pyc differ diff --git a/logs/test.py b/logs/test.py new file mode 100644 index 0000000..1e0aae1 --- /dev/null +++ b/logs/test.py @@ -0,0 +1,7 @@ +from scipy.spatial.transform import Rotation as R +import numpy as np + +arm_orientation_set0 = np.array([-180,-30,0]) +b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix() +tcp_offset = b_rotation_s_set0 @ np.array([0,0,154.071]) +print(tcp_offset) \ No newline at end of file