From 2dfb5f8447900f58a67855bfa82bf1276b0fc057 Mon Sep 17 00:00:00 2001 From: "ziwei.he" Date: Fri, 9 May 2025 14:03:24 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=9B=E6=8E=A7=E4=B8=AD=E9=97=B4=E5=B1=82?= =?UTF-8?q?=E9=80=82=E9=85=8D=E6=96=B0=E7=9A=84=E5=BA=95=E5=B1=82=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3=EF=BC=8C=E5=BC=80=E5=8F=91=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5 | 151 ++++++++++++++++++ .../algorithms/admittance_controller.py | 122 ++++++++++++++ .../MassageControl/algorithms/arm_state.py | 44 +++++ .../algorithms/base_controller.py | 80 ++++++++++ .../algorithms/controller_manager.py | 42 +++++ Massage/MassageControl/config/admittance.yaml | 66 ++++++++ .../config/massage_head/none_playload.yaml | 28 ++++ .../MassageControl/config/robot_config.yaml | 12 ++ .../MassageControl/hardware/dobot_nova5.py | 8 + Massage/MassageControl/tools/Rate.py | 80 ++++++++++ Massage/MassageControl/tools/log.py | 122 ++++++++++++++ Massage/MassageControl/tools/yaml_operator.py | 52 ++++++ logs/MassageRobot_nova5.log | 0 13 files changed, 807 insertions(+) create mode 100644 Massage/MassageControl/MassageRobot_nova5 create mode 100644 Massage/MassageControl/algorithms/admittance_controller.py create mode 100644 Massage/MassageControl/algorithms/arm_state.py create mode 100644 Massage/MassageControl/algorithms/base_controller.py create mode 100644 Massage/MassageControl/algorithms/controller_manager.py create mode 100644 Massage/MassageControl/config/admittance.yaml create mode 100644 Massage/MassageControl/config/massage_head/none_playload.yaml create mode 100644 Massage/MassageControl/config/robot_config.yaml create mode 100644 Massage/MassageControl/tools/Rate.py create mode 100644 Massage/MassageControl/tools/log.py create mode 100644 Massage/MassageControl/tools/yaml_operator.py create mode 100644 logs/MassageRobot_nova5.log diff --git a/Massage/MassageControl/MassageRobot_nova5 b/Massage/MassageControl/MassageRobot_nova5 new file mode 100644 index 0000000..e0696e4 --- /dev/null +++ b/Massage/MassageControl/MassageRobot_nova5 @@ -0,0 +1,151 @@ +from hardware.dobot_nova5 import dobot_nova5 + +from algorithms.arm_state import ArmState +from algorithms.controller_manager import ControllerManager +from algorithms.admittance_controller import AdmittanceController + +from tools.log import CustomLogger +from tools.yaml_operator import read_yaml +from tools.Rate import Rate +import numpy as np +import threading +import time +import os +class MassageRobot: + def __init__(self,arm_config_path,is_log=False): + self.logger = CustomLogger( + log_name="测试日志", + log_file="logs/MassageRobot_nova5_test.log", + precise_time=True) + if is_log: + self.logger_data = CustomLogger(log_name="运动数据日志", + log_file="logs/MassageRobot_kinematics_data.log", + precise_time=True) + # 日志线程 + threading.Thread(target=self.log_thread,daemon=True).start() + + self.arm_state = ArmState() + self.arm_config = read_yaml(arm_config_path) + self.arm = dobot_nova5(arm_ip=self.arm_config['arm_ip']) + self.force_sensor = None + + # controller + self.controller_manager = ControllerManager(self.arm_state) + self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0]) + + # massage heads + 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.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']) + + # 低通滤波 + self.cutoff_freq = 80.0 + + # flags + self.exit_event = threading.Event() + self.exit_event.set() # 运行 True + self.interrupt_event = threading.Event() + self.interrupt_event.clear() # 中断 False + self.pause_envent = threading.Event() + self.pause_envent.clear() # 暂停 False + self.skip_envent = threading.Event() + self.skip_envent.clear() # 跳过 False + + self.is_waitting = False + + self.last_print_time = 0 + 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) + # 过程噪声协方差矩阵 + self.Q_base = np.eye(6) * 0.01 + # 测量噪声协方差矩阵 + self.R_base = np.eye(6) * 0.1 + + self.x_tcp = np.zeros(6) + self.P_tcp = np.eye(6) + # 过程噪声协方差矩阵 + self.Q_tcp = np.eye(6) * 0.01 + # 测量噪声协方差矩阵 + self.R_tcp = np.eye(6) * 0.1 + # 传感器故障计数器 + self.sensor_fail_count = 0 + + # init + self.arm.__init__() + + # 预测步骤 + 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) + # 更新状态 + x_update = x_predict + K @ (z - x_predict) + # 更新误差协方差 + P_update = (np.eye(len(K)) - K) @ P_predict + return x_update, P_update + + def arm_measure_loop(self): + return + + def arm_command_loop(self): + return + + 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) + self.arm_measure_thread.start() + + + 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() + + return + + def init_hardwares(self,ready_pose): + self.ready_pose = np.array(ready_pose) + + def switch_payload(self,name): + if name in self.playload_dict: + return + + + def log_thread(self): + while True: + self.logger_data.log_info(f"机械臂位置:{self.arm_state.arm_position},机械臂姿态:{self.arm_state.arm_orientation}",is_print=False) + self.logger_data.log_info(f"机械臂期望位置:{self.arm_state.desired_position},机械臂真实位置:{self.arm_state.arm_position}",is_print=False) + self.logger_data.log_info(f"机械臂期望姿态:{self.arm_state.desired_orientation},机械臂真实姿态:{self.arm_state.arm_orientation}",is_print=False) + self.logger_data.log_info(f"机械臂期望力矩:{self.arm_state.desired_wrench},机械臂真实力矩:{self.arm_state.external_wrench_base}",is_print=False) + self.logger_data.log_info(f"当前按摩头:{self.current_head}",is_print=False) + time.sleep(1) + +if __name__ == "__main__": + robot = MassageRobot(arm_config_path="MassageControl/config/robot_config.yaml") \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py new file mode 100644 index 0000000..3034ea0 --- /dev/null +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -0,0 +1,122 @@ +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 +sys.path.append(str(Path(__file__).resolve().parent.parent)) +import time + +from tools.yaml_operator import read_yaml +class AdmittanceController(BaseController): + def __init__(self, name, state:ArmState,config_path) -> None: + super().__init__(name, state) + self.load_config(config_path) + + + 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']}") + mass_tran = np.array(config_dict['mass_tran']) + mass_rot = np.array(config_dict['mass_rot']) + stiff_tran = np.array(config_dict['stiff_tran']) + stiff_rot = np.array(config_dict['stiff_rot']) + desired_xi = np.array(config_dict['desired_xi']) + damp_tran = np.array(config_dict['damp_tran']) + damp_rot = np.array(config_dict['damp_rot']) + self.pos_scale_factor = config_dict['pos_scale_factor'] + self.rot_scale_factor = config_dict['rot_scale_factor'] + for i in range(3): + if damp_tran[i] < 0: + damp_tran[i] = 2 * desired_xi * np.sqrt(stiff_tran[i] * mass_tran[i]) + if damp_rot[i] < 0: + damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i]) + self.M = np.diag(np.concatenate([mass_tran, mass_rot])) + self.K = np.diag(np.concatenate([stiff_tran, stiff_rot])) + self.D = np.diag(np.concatenate([damp_tran, damp_rot])) + + self.laset_print_time = 0 + + def step(self,dt): + # 计算误差 位置直接作差，姿态误差以旋转向量表示 + + temp_pose_error = self.state.arm_position - self.state.desired_position + # if time.time() - self.laset_print_time > 5: + # print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}') + if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + self.state.arm_orientation = -self.state.arm_orientation + + self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error + # if time.time() - self.laset_print_time > 5: + # print("pose_error:",self.state.pose_error[:3]) + # 计算误差 位置直接作差，姿态误差以旋转向量表示 + #rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T + rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix() + # print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}') + rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False) + self.state.pose_error[3:] = -rot_err_rotvex + + #wrench_err = self.state.external_wrench_base - self.state.desired_wrench + wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + # if time.time() - self.laset_print_time > 5: + # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') + self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) + # if time.time() - self.laset_print_time > 5: + # print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_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 + + delta_pose[:3] = self.pos_scale_factor * delta_pose[:3] + delta_pose[3:] = self.rot_scale_factor * delta_pose[3:] + # if time.time() - self.laset_print_time > 5: + # print("delta_pose:",delta_pose) + + delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3] + + # if time.time() - self.laset_print_time > 5: + # print("tf_delta_pose:",delta_pose) + self.clip_command(delta_pose,"pose") + + # testlsy + 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() + arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat + # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() + + 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) + + + # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:] + # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat() + + # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() + self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + # if time.time() - self.laset_print_time > 0.1: + # print("-------------admittance_1-------------------------------") + # 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 __name__ == "__main__": + state = ArmState() + controller = AdmittanceController("admittance",state,"/home/zyc/admittance_control/MassageControl/config/admittance.yaml") + print(controller.name) + print(controller.state.arm_position) + state.arm_position = np.array([1,2,3]) + print(controller.state.arm_position) + print(controller.M) + print(controller.D) + print(controller.K) \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/arm_state.py b/Massage/MassageControl/algorithms/arm_state.py new file mode 100644 index 0000000..3d28154 --- /dev/null +++ b/Massage/MassageControl/algorithms/arm_state.py @@ -0,0 +1,44 @@ +import numpy as np + +class ArmState: + def __init__(self) -> None: + # 当前状态 + + self.arm_position = np.zeros(3,dtype=np.float64) + self.arm_orientation = np.array([0.0,0.0,0.0,1.0]) # [qx, qy, qz, qw] + self.external_wrench_base = np.zeros(6,dtype=np.float64) + self.external_wrench_tcp = np.zeros(6,dtype=np.float64) + + # 上一个状态 + self.last_arm_position = np.zeros(3,dtype=np.float64) + self.last_arm_orientation = np.array([0.0,0.0,0.0,1.0]) # [qx, qy, qz, qw] + self.last_external_wrench_base = np.zeros(6,dtype=np.float64) + self.last_external_wrench_tcp = np.zeros(6,dtype=np.float64) + + # 目标状态 + self.desired_position = np.zeros(3,dtype=np.float64) + self.desired_orientation = np.array([0.0,0,0,1]) # [qx, qy, qz, qw] + self.desired_wrench = np.zeros(6,dtype=np.float64) + self.desired_twist = np.zeros(6,dtype=np.float64) + + # 导纳计算过程变量 + self.arm_desired_twist = np.zeros(6,dtype=np.float64) + self.arm_desired_twist_tcp = np.zeros(6,dtype=np.float64) + self.arm_desired_acc = np.zeros(6,dtype=np.float64) + + # 控制输出 + self.arm_position_command = np.zeros(3,dtype=np.float64) + self.arm_orientation_command = np.array([0,0,0,1]) # [qx, qy, qz, qw] + + # 误差信息 + self.pose_error = np.zeros(6,dtype=np.float64) + self.twist_error = np.zeros(6,dtype=np.float64) + self.wrench_error = np.zeros(6,dtype=np.float64) + + # clip项 + self.max_acc_tran = 40 + self.max_acc_rot = 20 * 3.5 + self.max_vel_tran = 0.5 * 2.1 + self.max_vel_rot = 3.1415 * 2 #1.5 + self.max_dx = 0.01 *1.5 + self.max_dr = 0.0087 * 3*1.5 \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/base_controller.py b/Massage/MassageControl/algorithms/base_controller.py new file mode 100644 index 0000000..76e0ef3 --- /dev/null +++ b/Massage/MassageControl/algorithms/base_controller.py @@ -0,0 +1,80 @@ +from abc import ABC, abstractmethod +from typing import Literal +import numpy as np +from scipy.spatial.transform import Rotation as R +from .arm_state import ArmState + +""" + 位置单位为m，力单位为N，力矩单位为Nm，角度单位为rad +""" +class BaseController(ABC): + def __init__(self,name,state:ArmState) -> None: + super().__init__() + self.name = name + self.state = state + + + @abstractmethod + def step(self,dt): + # 算法的一次迭代 + pass + + @abstractmethod + def load_config(self, config_path): + # 加载配置文件 + pass + + def clip_command(self, command :np.array,type: Literal["acc", "vel", "pose"],is_print=False): + if type == "acc": + if np.linalg.norm(command[:3]) > self.state.max_acc_tran: + command[:3] = command[:3] / np.linalg.norm(command[:3]) * self.state.max_acc_tran + if is_print: + print(f"translational acceleration {np.linalg.norm(command[:3])}m/s exceeds maximum allowed value") + if np.linalg.norm(command[3:]) > self.state.max_acc_rot: + if is_print: + print(f"rotational acceleration {np.linalg.norm(command[3:])}rad/s exceeds maximum allowed value,") + command[3:] = command[3:] / np.linalg.norm(command[3:]) * self.state.max_acc_rot + elif type == "vel": + if np.linalg.norm(command[:3]) > self.state.max_vel_tran: + if is_print: + print(f"translational velocity {np.linalg.norm(command[:3])}m/s exceeds maximum allowed value,") + command[:3] = command[:3] / np.linalg.norm(command[:3]) * self.state.max_vel_tran + if np.linalg.norm(command[3:]) > self.state.max_vel_rot: + command[3:] = command[3:] / np.linalg.norm(command[3:]) * self.state.max_vel_rot + if is_print: + print(f"rotational velocity {np.linalg.norm(command[3:])}rad/s exceeds maximum allowed value") + elif type == "pose": + if np.linalg.norm(command[:3]) > self.state.max_dx: + command[:3] = command[:3] / np.linalg.norm(command[:3]) * self.state.max_dx + if is_print: + print(f"translational displacement {np.linalg.norm(command[:3])}m exceeds maximum allowed value") + if np.linalg.norm(command[3:]) > self.state.max_dr: + command[3:] = command[3:] / np.linalg.norm(command[3:]) * self.state.max_dr + if is_print: + print(f"rotational displacement {np.linalg.norm(command[3:])}rad exceeds maximum allowed value") + + + + @staticmethod + def rotvec_pose_add(pose, delta_pose): + """ + Compute the pose sum between two poses, which consists if position (x, y, z) and rotation vector (rx, ry, rz). + Update rule: x_t+1 = x_t + dx, R_t+1 = dR * R_t (rotation matrix) + + :param pose: np.ndarray (6,) + :param delta_pose: np.ndarray (6,) + :return: np.ndarray (6,) + """ + assert len(pose) == 6 and len(delta_pose) == 6, "pose and delta_pose must be 6-dimensional" + + ret = np.zeros(6) + ret[:3] = pose[:3] + delta_pose[:3] + # 当前姿态的旋转矩阵 + pose_matrix = R.from_rotvec(pose[3:]).as_matrix() + # 旋转矩阵的增量 + pose_delta_matrix = R.from_rotvec(delta_pose[3:]).as_matrix() + # 更新后的旋转矩阵，然后转换为旋转向量 + ret[3:] = R.from_matrix(pose_delta_matrix @ pose_matrix).as_rotvec() + return ret + + \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/controller_manager.py b/Massage/MassageControl/algorithms/controller_manager.py new file mode 100644 index 0000000..9d755db --- /dev/null +++ b/Massage/MassageControl/algorithms/controller_manager.py @@ -0,0 +1,42 @@ +from .arm_state import ArmState + +class ControllerManager: + def __init__(self, state: ArmState): + self.state = state + self.controllers = {} + self.current_controller = None + + def add_controller(self, controller_class, name, config_path): + if name not in self.controllers: + self.controllers[name] = controller_class(name,self.state,config_path) + else: + raise ValueError(f"Controller {name} already exists") + + def remove_controller(self, name): + if name in self.controllers: + del self.controllers[name] + if self.current_controller == self.controllers.get(name): + self.current_controller = None + else: + raise ValueError(f"Controller {name} does not exist") + + def switch_controller(self, name): + if name in self.controllers: + self.current_controller = self.controllers[name] + else: + raise ValueError(f"Controller {name} does not exist") + + def __getattr__(self, name): + if self.current_controller: + method = getattr(self.current_controller, name, None) + if method: + return method + else: + raise AttributeError(f"'{type(self.current_controller).__name__}' object has no attribute '{name}'") + else: + raise RuntimeError("No current controller is set") + + + def step(self,dt): + if self.current_controller: + self.current_controller.step(dt) \ No newline at end of file diff --git a/Massage/MassageControl/config/admittance.yaml b/Massage/MassageControl/config/admittance.yaml new file mode 100644 index 0000000..558fc54 --- /dev/null +++ b/Massage/MassageControl/config/admittance.yaml @@ -0,0 +1,66 @@ +name: admittance +# mass_tran: [5,5,5] +# mass_rot: [5,5,5] +# stiff_tran: [500,500,500] +# stiff_rot: [500,500,500] +# desired_xi: 0.7 +# damp_tran: [-125,-125,-125] +# damp_rot: [-125,-125,-125] +# pos_scale_factor: 1 +# rot_scale_factor: 1 + +# name: admittance +# mass_tran: [1,1,1] +# mass_rot: [0.5,0.5,0.5] +# stiff_tran: [800,800,800] +# stiff_rot: [100,100,100] +# desired_xi: 1.1 +# damp_tran: [-2,-2,-2] +# damp_rot: [-10,-10,-10] +# pos_scale_factor: 1 +# rot_scale_factor: 1 + +# mass_tran: [4,4,4] +# mass_rot: [0.4,0.4,0.4] +# stiff_tran: [500,500,500] +# stiff_rot: [6,6,6] +# desired_xi: 1.1 +# damp_tran: [-1,-1,-1] +# damp_rot: [-1,-1,-1] +# pos_scale_factor: 1 +# rot_scale_factor: 12 + +# mass_tran: [1.3,0.4,2.4] +# mass_rot: [0.08,0.02,0.01] +# stiff_tran: [200,200,200] +# stiff_rot: [200,200,200] +# desired_xi: 1.1 +# damp_tran: [-1,-1,-1] +# damp_rot: [-1,-1,-1] +# pos_scale_factor: 1 +# rot_scale_factor: 1 + +desired_xi: 1 +pos_scale_factor: 1 +rot_scale_factor: 1 + +# mass_tran: [2.0, 2.0, 2.0] +# mass_rot: [0.2, 0.2, 0.2] +# stiff_tran: [300, 300, 300] +# stiff_rot: [3, 3, 3] +# damp_tran: [30, 30, 30] +# damp_rot: [1.0, 1.0, 1.0] + + + +mass_tran: [2.0, 2.0, 2.0] +mass_rot: [0.2, 0.2, 0.2] +stiff_tran: [400, 400, 400] +stiff_rot: [4, 4, 4] +# stiff_tran: [100, 100, 100] +# stiff_rot: [1, 1, 1] +damp_tran: [20, 20, 20] +damp_rot: [0.6, 0.6, 0.6] +# damp_tran: [40, 40, 40] +# damp_rot: [3, 3, 3] + diff --git a/Massage/MassageControl/config/massage_head/none_playload.yaml b/Massage/MassageControl/config/massage_head/none_playload.yaml new file mode 100644 index 0000000..bc5f928 --- /dev/null +++ b/Massage/MassageControl/config/massage_head/none_playload.yaml @@ -0,0 +1,28 @@ +name: none +sensor_mass: 0.334 +tool_type: Cylinder +tcp_offset: +- 0 +- 0 +- 0.031 +- 0 +- 0 +- 0 +tool_radius: 0.04 +tool_mass: 0 +mass_center_position: +- 0.021919029914178913 +- -0.010820480799427892 +- 0.011034252651402962 +force_zero: +- 0.8846671183185211 +- -0.6473878547983709 +- -2.1312346218888862 +torque_zero: +- 0.017893715524241308 +- 0.04546799757174578 +- -0.029532236049108707 +gravity_base: +- 0.9041730658541057 +- 1.6570854791729466 +- -1.8745612276068087 \ No newline at end of file diff --git a/Massage/MassageControl/config/robot_config.yaml b/Massage/MassageControl/config/robot_config.yaml new file mode 100644 index 0000000..f18a709 --- /dev/null +++ b/Massage/MassageControl/config/robot_config.yaml @@ -0,0 +1,12 @@ +# IP for DOBOT NOVA5 +arm_ip: '192.168.5.1' + +# controller +controller: ['Massage/MassageControl/config/admittance.yaml'] + +# massage heads diretory +massage_head_dir: 'Massage/MassageControl/config/massage_head' + +control_rate: 50 +sensor_rate: 100 +command_rate: 120 \ No newline at end of file diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index dbd22d2..4a4b6c1 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -323,6 +323,14 @@ class dobot_nova5: else: print("下使能机械臂失败") return + + def clearError(self): + code = self.dashboard.ClearError() + if code == 0: + print("清楚报警成功") + else: + print("清楚报警失败") + return def __del__(self): del self.dashboard diff --git a/Massage/MassageControl/tools/Rate.py b/Massage/MassageControl/tools/Rate.py new file mode 100644 index 0000000..eeba960 --- /dev/null +++ b/Massage/MassageControl/tools/Rate.py @@ -0,0 +1,80 @@ +import time +import asyncio + +class Rate: + def __init__(self, hz): + self.interval = 1.0 / hz + self.last_time = time.monotonic() + self.start_time = self.last_time + + def sleep(self,print_duration=False): + now = time.monotonic() + sleep_duration = self.interval - (now - self.last_time) + if sleep_duration > 0: + if print_duration: + print(f"睡眠时间: {sleep_duration:.4f} 秒") + time.sleep(sleep_duration) + self.last_time = time.monotonic() + + def precise_sleep(self): + # 计算距离下一次休眠的剩余时间 + now = time.perf_counter() + elapsed = now - self.last_time + sleep_duration = self.interval - elapsed + + # 分成多个小的睡眠周期 + while sleep_duration > 0: + # 最大睡眠时间限制在 1ms 以内，避免过多的误差 + sleep_time = min(sleep_duration, 0.001) # 最大休眠1ms + time.sleep(sleep_time) + now = time.perf_counter() + elapsed = now - self.last_time + sleep_duration = self.interval - elapsed + + self.last_time = time.perf_counter() + + async def async_sleep(self): + now = time.monotonic() + sleep_duration = self.interval - (now - self.last_time) + if sleep_duration > 0: + # print(f"睡眠时间: {sleep_duration:.4f} 秒") + await asyncio.sleep(sleep_duration) + self.last_time = time.monotonic() + + def remaining(self): + now = time.monotonic() + remaining_time = self.interval - (now - self.last_time) + return max(0.0, remaining_time) + + def cycle_time(self): + now = time.monotonic() + cycle_duration = now - self.start_time + self.start_time = now + return cycle_duration + + def to_sec(self): + return self.interval + +if __name__ == "__main__": + # 示例用法：同步代码 + def main_sync(): + rate = Rate(2) # 2 赫兹的循环频率（每次迭代0.5秒） + for i in range(10): + print(f"同步迭代 {i}") + print(f"剩余时间: {rate.remaining():.4f} 秒") + print(f"上一个循环周期时间: {rate.cycle_time():.4f} 秒") + rate.sleep() + + # 示例用法：异步代码 + async def main_async(): + rate = Rate(2) # 2 赫兹的循环频率（每次迭代0.5秒） + for i in range(10): + print(f"异步迭代 {i}") + print(f"剩余时间: {rate.remaining():.4f} 秒") + print(f"上一个循环周期时间: {rate.cycle_time():.4f} 秒") + await rate.async_sleep() + + main_sync() + + asyncio.run(main_async()) + diff --git a/Massage/MassageControl/tools/log.py b/Massage/MassageControl/tools/log.py new file mode 100644 index 0000000..eb3fc4e --- /dev/null +++ b/Massage/MassageControl/tools/log.py @@ -0,0 +1,122 @@ +import json +import logging +from colorama import Fore, Style, init +from datetime import datetime +import numpy as np +import sys +import inspect +import os + +# 初始化 colorama（自动重置颜色） +init(autoreset=True) + +class CustomLogger: + def __init__(self, log_name=None, log_file=None, propagate=False, precise_time=True): + """初始化日志记录器 + + :param log_name: 日志名称标识 + :param propagate: 是否传递日志到父记录器 + :param precise_time: 是否启用毫秒级时间戳 + """ + self.logger = logging.getLogger(f"custom_logger_{log_name}") + self.logger.setLevel(logging.INFO) + self.logger.propagate = propagate + self.log_name = log_name + self.precise_time = precise_time + # 配置日志格式器 + self.log_formatter = logging.Formatter( + '%(asctime)s - %(log_name)s - %(levelname)s - %(filename)s:%(lineno)d - %(message)s' + ) + + # 默认添加控制台处理器 + if not self.logger.handlers: + console_handler = logging.StreamHandler() + console_handler.setFormatter(self.log_formatter) + self.logger.addHandler(console_handler) + # 添加文件处理器 + if log_file: + # 确保日志目录存在 + os.makedirs(os.path.dirname(log_file), exist_ok=True) + file_handler = logging.FileHandler(log_file, encoding='utf-8') + file_handler.setFormatter(self.log_formatter) + self.logger.addHandler(file_handler) + + def __enter__(self): + """上下文管理器入口""" + return self + + def __exit__(self, exc_type, exc_value, traceback): + """上下文管理器退出时清理资源""" + for handler in self.logger.handlers[:]: + self.logger.removeHandler(handler) + handler.close() + + # 彩色日志方法 + def log_info(self, message, is_print=True): + self._log(message, logging.INFO, Fore.GREEN, is_print) + + def log_yellow(self, message, is_print=True): + self._log(message, logging.INFO, Fore.YELLOW, is_print) + + def log_blue(self, message, is_print=True): + self._log(message, logging.INFO, Fore.CYAN, is_print) + + def log_warning(self, message, is_print=True): + self._log(message, logging.WARNING, Fore.YELLOW, is_print) + + def log_error(self, message, is_print=True): + self._log(message, logging.ERROR, Fore.RED, is_print) + + def _log(self, message, level, color, is_print=True): + """内部日志处理核心方法 + + :param message: 日志内容（支持字符串/字典/数组等） + :param level: 日志级别（如 logging.INFO） + :param color: colorama 颜色控制符 + :param is_print: 是否打印到控制台 + """ + # 1. 时间戳格式化 + current_time = datetime.now() + if self.precise_time: + formatted_time = current_time.strftime('%Y-%m-%d %H:%M:%S.') + f"{current_time.microsecond // 1000:03d}" + else: + formatted_time = current_time.strftime('%Y-%m-%d %H:%M:%S') + + # 2. 获取调用者信息（跳过两层栈帧） + caller_frame = inspect.stack()[2] + filename = os.path.basename(caller_frame.filename) + lineno = caller_frame.lineno + + # 3. 序列化复杂数据类型 + if isinstance(message, (dict, list, tuple, np.ndarray)): + try: + message = json.dumps(message if not isinstance(message, np.ndarray) else message.tolist(), + ensure_ascii=False) + except (TypeError, ValueError): + message = str(message) + else: + message = str(message) + + # 4. 控制台彩色输出 + if is_print: + level_name = logging.getLevelName(level) + print(f"{formatted_time} - {self.log_name} - {level_name} - {filename}:{lineno} - {color}{message}{Style.RESET_ALL}") + + # 5. 写入日志系统 + self.logger.log(level, message, extra={'log_name': self.log_name}) + +if __name__ == "__main__": + # 示例用法 + with CustomLogger( + log_name="测试日志", + log_file="logs/MassageRobot_nova5.log", + precise_time=True + ) as logger: + logger.log_info("普通信息（绿色）") + logger.log_yellow("黄色提示") + logger.log_blue("蓝色消息") + logger.log_warning("警告信息（黄色）") + logger.log_error("错误信息（红色）") + # 测试复杂数据类型 + # logger.log_info({"key": "值", "数组": [1, 2, 3]}) + # logger.log_info(np.random.rand(3, 3)) \ No newline at end of file diff --git a/Massage/MassageControl/tools/yaml_operator.py b/Massage/MassageControl/tools/yaml_operator.py new file mode 100644 index 0000000..2f11dc9 --- /dev/null +++ b/Massage/MassageControl/tools/yaml_operator.py @@ -0,0 +1,52 @@ +import yaml + +def read_yaml(file_path): + """ + 读取 YAML 文件并返回 Python 对象。 + + 参数: + file_path (str): YAML 文件路径。 + + 返回: + data (dict): YAML 文件内容转换的 Python 对象。 + """ + with open(file_path, 'r', encoding='utf-8') as file: + data = yaml.safe_load(file) + return data + +def write_yaml(data, file_path): + """ + 将 Python 对象写入 YAML 文件。 + + 参数: + data (dict): 要写入 YAML 文件的 Python 对象。 + file_path (str): 目标 YAML 文件路径。 + """ + with open(file_path, 'w', encoding='utf-8') as file: + yaml.safe_dump(data, file, default_flow_style=False, allow_unicode=True, sort_keys=False) + +def update_yaml(file_path, key, value): + """ + 更新 YAML 文件中的指定键值对。 + + 参数: + file_path (str): YAML 文件路径。 + key (str): 要更新的键。 + value: 要更新的值。 + """ + data = read_yaml(file_path) + data[key] = value + write_yaml(data, file_path) + +def delete_key_yaml(file_path, key): + """ + 删除 YAML 文件中的指定键。 + + 参数: + file_path (str): YAML 文件路径。 + key (str): 要删除的键。 + """ + data = read_yaml(file_path) + if key in data: + del data[key] + write_yaml(data, file_path) \ No newline at end of file diff --git a/logs/MassageRobot_nova5.log b/logs/MassageRobot_nova5.log new file mode 100644 index 0000000..e69de29