Merge branch 'main' of https://git.robotstorm.tech/DevelopmentTeam/MassageRobot_Dobot into main

Massage/MassageControl/algorithms/admithybrid_controller.py

@@ -0,0 +1,135 @@
+import sys
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from .arm_state import ArmState
+from .base_controller import BaseController
+from pathlib import Path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+from tools.yaml_operator import read_yaml
+import time
+
+class AdmitHybridController(BaseController):
+    def __init__(self, name, state: ArmState,config_path) -> None:
+        super().__init__(name, state)
+        self.load_config(config_path)
+        self.laset_print_time = 0
+    
+
+    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']}")
+        desired_xi = np.array(config_dict['desired_xi'])
+        damp_tran = np.array(config_dict['damp_tran'])
+        # TODO 修改控制率
+        self.e_t = 0
+        self.e_t1 = 0
+        self.e_t2 = 0
+        self.force_control_value = 0
+
+        # 姿态 3x3矩阵
+        self.Kp = np.diag(np.array(config_dict['Kp']))
+        self.Ki = np.diag(np.array(config_dict['Ki']))
+        self.Kd = np.diag(np.array(config_dict['Kd']))
+
+        self.pose_integral_error = np.zeros(6)
+
+        # 导纳xyz位置调节器
+        mass_tran = np.array(config_dict['mass_tran'])
+        stiff_tran = np.array(config_dict['stiff_tran'])
+        damp_tran = np.array(config_dict['damp_tran'])
+        for i in range(3):
+            if damp_tran[i] < 0:
+                damp_tran[i] = 2 * desired_xi * np.sqrt(stiff_tran[i] * mass_tran[i])
+        self.M_tran = np.diag(mass_tran)
+        self.M_tran_inv = np.linalg.inv(self.M_tran)
+        self.K_tran = np.diag(stiff_tran)
+        self.D_tran = np.diag(damp_tran)
+
+    def step(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        temp_pose_error = self.state.arm_position - self.state.desired_position
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+        # 姿态误差(四元数)
+        rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd @ self.state.arm_desired_twist[3:] - self.Kp @ self.state.pose_error[3:] - self.Ki @ self.pose_integral_error[3:]
+        # 位置导纳控制
+        self.state.arm_desired_acc[:3] = self.M_tran_inv @ (wrench_err[:3] - self.D_tran @ (self.state.arm_desired_twist[:3] - self.state.desired_twist[:3])-self.K_tran @ self.state.pose_error[:3])
+
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+    def step_traj(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        
+        temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+
+        # 姿态误差(四元数)
+        angular_velocity = np.array(self.state.desired_twist[3:])  # 形状 (3,)
+
+        # 用旋转向量（小角度近似）
+        rotvec = angular_velocity * dt  # 旋转向量 = 角速度 × 时间
+        rot_quat = R.from_rotvec(rotvec).as_quat()  # 转成四元数，形状 (4,)
+        rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd @ self.state.arm_desired_twist[3:] - self.Kp @ self.state.pose_error[3:] - self.Ki @ self.pose_integral_error[3:]
+        # 位置导纳控制
+        self.state.arm_desired_acc[:3] = self.M_tran_inv @ (wrench_err[:3] - self.D_tran @ (self.state.arm_desired_twist[:3] - self.state.desired_twist[:3] + self.state.desired_acc[:3]*dt)-self.K_tran @ self.state.pose_error[:3]) + self.state.desired_acc[:3]
+
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]

Massage/MassageControl/algorithms/admittanceZ_controller.py

@@ -0,0 +1,153 @@
+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 AdmittanceZController(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(2):
+            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])
+
+        if damp_rot[2] < 0:
+            damp_rot[2] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i])
+
+        self.M_trans = np.diag(mass_tran[:2])
+        self.M_rot = np.diag(mass_rot)
+        self.M_z = mass_tran[2]
+        self.M = np.diag(np.concatenate([mass_tran, mass_rot]))
+        self.M_trans_inv = np.linalg.inv(self.M_trans)
+        self.M_rot_inv = np.linalg.inv(self.M_rot)
+
+        self.K_trans = np.diag(stiff_tran[:2])
+        self.K_rot = np.diag(stiff_rot)
+        self.K_z = stiff_tran[2]
+        self.K_trans_inv = np.linalg.inv(self.K_trans)
+        self.K_rot_inv = np.linalg.inv(self.K_rot)
+        self.D_trans = np.diag(damp_tran[:2])
+        self.D_rot = np.diag(damp_rot)
+        self.D_z = damp_tran[2]
+
+        self.laset_print_time = 0
+
+        self.pose_integral_error = np.zeros(6)
+
+    def step(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        temp_pose_error = self.state.arm_position - self.state.desired_position
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+        # 姿态误差(四元数)
+        rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+        self.state.arm_desired_acc[:2] = self.M_trans_inv @ (wrench_err[:2] - self.D_trans @ (self.state.arm_desired_twist[:2] -self.state.desired_twist[:2]) - self.K_trans @ self.state.pose_error[:2])
+        self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] -self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2])
+        self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] -self.state.desired_twist[3:]) - self.K_rot @ self.state.pose_error[3:])
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+    def step_traj(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        
+        temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+
+        # 姿态误差(四元数)
+        angular_velocity = np.array(self.state.desired_twist[3:])  # 形状 (3,)
+
+        # 用旋转向量（小角度近似）
+        rotvec = angular_velocity * dt  # 旋转向量 = 角速度 × 时间
+        rot_quat = R.from_rotvec(rotvec).as_quat()  # 转成四元数，形状 (4,)
+        rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+        self.state.arm_desired_acc[:2] = self.M_trans_inv @ (wrench_err[:2] - self.D_trans @ (self.state.arm_desired_twist[:2] -self.state.desired_twist[:2] + self.state.desired_acc[:2]*dt) - self.K_trans @ self.state.pose_error[:2]) + self.state.desired_acc[:2]
+        self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] -self.state.desired_twist[2] + self.state.desired_acc[2]*dt) - self.K_z * self.state.pose_error[2]) + self.state.desired_acc[2]
+        self.state.arm_desired_acc[3:] = self.M_rot_inv @ (wrench_err[3:] - self.D_rot @ (self.state.arm_desired_twist[3:] -self.state.desired_twist[3:] + self.state.desired_acc[3:]*dt) - self.K_rot @ self.state.pose_error[3:]) + self.state.desired_acc[3:]
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+    
+
+
+
+if __name__ == "__main__":
+    state = ArmState()
+    controller = AdmittanceZController("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)

Massage/MassageControl/algorithms/arm_state.py

@@ -15,6 +15,10 @@ class ArmState:
         self.last_external_wrench_base = np.zeros(6,dtype=np.float64)
         self.last_external_wrench_tcp = np.zeros(6,dtype=np.float64)
 
+        # 传感器数据
+        self.positionerSensorData = 0.0
+        self.desired_high = 0.06
+        
         # 目标状态
         self.desired_position = np.zeros(3,dtype=np.float64)
         self.desired_orientation = np.array([0.0,0,0,1]) # [qx, qy, qz, qw]

Massage/MassageControl/algorithms/force_planner.py

@@ -0,0 +1,273 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import copy
+
+class ForcePlanner:
+    def __init__(self):
+        self.force_vel_max = np.float64(20) # [0,0, 10, 0, 0, 0] # 10 N/s
+        self.force_acc_max = np.float64(10) #[0,0, 20, 0, 0, 0] # 20 N/s²
+        self.force_jerk_max = 90 #[0,0, 30, 0, 0, 0] # 30 N/s³
+        self.Ta = self.force_vel_max / self.force_acc_max
+
+    def linear_wrench_interpolate(self,wrench=None, time_points=None, time_step=0.1):
+        if wrench is None:
+            raise ValueError("至少需要输入力矩的序列")
+        if time_points is None or len(time_points) < 2:
+            raise ValueError("需要提供至少两个时间点")
+        
+        # 由于浮点数精度问题，time_step要乘0.9
+        times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step)
+
+        if wrench is not None:
+            all_wrenchs = np.zeros((len(times), wrench.shape[1]))
+            current_idx = 0
+            start_wrench =  np.array([0, 0, 0, 0, 0, 0])
+            end_wrench =  wrench
+
+            for i in range(len(times)):
+                # 计算 phase
+                phase = i / len(times)  # 使用 i 计算
+
+                # 计算 segment_wrenchs
+                segment_wrenchs = start_wrench + phase * (end_wrench - start_wrench)
+
+                # 将 segment_wrenchs 填充到 all_wrenchs
+                all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs
+                current_idx += len(segment_wrenchs)
+
+            # 确保最后一个位置被处理
+            if current_idx < len(times):
+                all_wrenchs[current_idx:] = wrench
+
+        all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -50, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0]))
+
+        if wrench is not None:
+            return np.array(all_wrenchs)
+
+    def oscillation_wrench_interpolate(self,wrench=None, time_points=None, time_step=0.1):
+        if wrench is None:
+            raise ValueError("至少需要输入力矩的序列")
+        if time_points is None or len(time_points) < 2:
+            raise ValueError("需要提供至少两个时间点")
+        
+        # 由于浮点数精度问题，time_step要乘0.9
+        times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step)
+
+        if wrench is not None:
+            all_wrenchs = np.zeros((len(times), wrench.shape[1]))
+            current_idx = 0
+
+
+            up_wrench = np.array([0, 0, 0, 0, 0, 0])
+            down_wrench = np.array([0, 0, 0, 0, 0, 0])
+            up_wrench[2] = wrench[0][2] + 15 if wrench[0][2] + 15 < 0 else 0
+            down_wrench[2] = wrench[0][2] - 15 if wrench[0][2] - 15 > -50 else -50
+
+
+            for i in range(len(times)):
+                # 计算 phase
+                phase = np.sin(20 * np.pi * i / len(times))  # 使用 i 计算
+
+                phase = 1 if phase >= 0 else -1 
+
+                # 计算 amplitude
+                # amplitude = np.array([0, 0, -50, 0, 0, 0]) if phase < 0 else np.array([0, 0, 0, 0, 0, 0])
+                amplitude = down_wrench if phase < 0 else up_wrench
+                # 计算 segment_wrenchs
+                segment_wrenchs = wrench + phase * (amplitude - wrench)
+
+                # 将 segment_wrenchs 填充到 all_wrenchs
+                all_wrenchs[current_idx:current_idx + len(segment_wrenchs)] = segment_wrenchs
+                current_idx += len(segment_wrenchs)
+
+            # 确保最后一个位置被处理
+            if current_idx < len(times):
+                all_wrenchs[current_idx:] = wrench
+
+        all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0]))
+
+        if wrench is not None:
+            return np.array(all_wrenchs)
+
+
+    def S_shaped_wrench_interpolate(self,start=None, wrench=None, time_points=None, time_step=0.1):
+        if wrench is None:
+            raise ValueError("At least a wrench sequence must be provided.")
+        if time_points is None or len(time_points) < 2:
+            raise ValueError("At least two time points are required.")
+        if start is None:
+            raise ValueError("At least a start wrench must be provided.")
+        Tave = time_points[-1] / len(wrench)
+        print("Tave:",Tave)
+
+        start_wrench = copy.deepcopy(np.array(start))
+        print("start_wrench:",start_wrench)
+        
+        # Time points array creation
+        times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step)
+        # wrench = [wrench]
+        wrench = np.array(wrench)
+
+        force_vel = 0
+        num = 0
+
+        all_wrenchs = np.zeros((len(times), 6))  # Use wrench.shape[0] to get the number of components
+        # 时间点的插值计算
+        for count in range(len(times)):
+            # 计算当前阶段的进度
+            phase = count / len(times)
+            plant = phase * times[-1]
+            # print("plant:",plant)
+
+            if plant >= num * Tave:
+                # print("plant:",plant,num,Tave)
+                if wrench[num][2] != 0:
+                    # Convert wrench to numpy array if it's a list
+                    coeff = 1
+                    end_wrench = copy.deepcopy(wrench[num])  # No need to convert again since wrench is now a numpy array
+                    print("end_wrench:",end_wrench,num)
+                    num += 1
+
+                    if np.abs(end_wrench[2]-start_wrench[2]) > self.force_vel_max*self.Ta:
+                        Ta = np.floor(self.Ta /time_step) * time_step
+                        self.force_vel = Ta * self.force_acc_max
+                        Tj = (np.abs(end_wrench[2]-start_wrench[2])-self.force_vel*Ta)/self.force_vel
+                        coeff = np.abs(end_wrench[2] - start_wrench[2]) / (np.floor((Ta + Tj)/time_step)*time_step * self.force_vel)
+                    else:
+                        Ta = np.floor(np.sqrt(np.abs(end_wrench[2]-start_wrench[2])/self.force_acc_max)/time_step)*time_step
+                        Tj = 0
+                        coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step)
+
+                    if Tave < 2*Ta + Tj:
+                        Ta = np.floor(Tave/time_step/2)*time_step
+                        Tj = 0
+                        coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step)
+                    
+                    if np.abs(end_wrench[2]-start_wrench[2]) < 0.1:
+                        coeff = 1
+                    
+                    start_wrench = np.array(start_wrench).astype(float)  # Ensure it's an array
+                    last_wrench = copy.deepcopy(start_wrench[2])
+                    force_vel = 0
+                    plant = 0
+                    segment_wrenchs = copy.deepcopy(start_wrench)
+                    dir = (end_wrench[2]-start_wrench[2])/np.abs(end_wrench[2]-start_wrench[2])
+                    t_start = copy.deepcopy(start_wrench)
+                else:
+
+                    coeff = 1
+                    end_wrench = copy.deepcopy(wrench[num]) 
+                    num += 1
+                    segment_wrenchs = copy.deepcopy(start_wrench)
+                    if Tave > 2:
+                        Ta = 1
+                    else:
+                        Ta = np.floor(Tave*10/2)/10
+                    Tj = 0
+                    force_vel = 0
+                    plant = 0
+                    coeff = np.abs(end_wrench[2] - start_wrench[2]) / ((self.force_acc_max*time_step) * (np.floor(Ta/time_step)*np.floor(Ta/time_step))*time_step)
+                    dir = (end_wrench[2]-start_wrench[2])/np.abs(end_wrench[2]-start_wrench[2])
+                    last_wrench = copy.deepcopy(start_wrench[2])
+                    force_vel = 0
+                    t_start = copy.deepcopy(start_wrench)
+
+                # print("all_wrenchs:", all_wrenchs.shape)
+
+            plant = phase * times[-1] - (num-1) * Tave
+
+            if end_wrench[2] != 0:
+                if plant == 0:
+                    segment_wrenchs = copy.deepcopy(start_wrench)
+                elif 0 < plant <= Ta:
+                    # 加速阶段
+                    force_vel += self.force_acc_max * time_step
+                    if force_vel > self.force_vel_max:
+                        force_vel = copy.deepcopy(self.force_vel_max)
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                elif Ta < plant <= Ta + Tj:
+                    # 恒速阶段
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                elif Ta + Tj < plant <= 2 * Ta + Tj:
+                    # 减速阶段
+                    force_vel -= self.force_acc_max * time_step
+                    if force_vel < 0:
+                        force_vel = 0
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                else:
+                    # 超过所有阶段后，使用结束力矩
+                    segment_wrenchs = segment_wrenchs
+                
+                all_wrenchs[count] = copy.deepcopy(segment_wrenchs) 
+                all_wrenchs[count][2] = (all_wrenchs[count][2] - t_start[2]) * coeff + t_start[2]
+                start_wrench = copy.deepcopy(all_wrenchs[count])
+            else:
+                if plant-(Tave-Tj-2*Ta) == 0:
+                    segment_wrenchs = copy.deepcopy(start_wrench)
+                elif 0 < plant-(Tave-Tj-2*Ta) <= Ta:
+                    # 加速阶段
+                    force_vel += self.force_acc_max * time_step
+                    if force_vel > self.force_vel_max:
+                        force_vel = copy.deepcopy(self.force_vel_max)
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                elif Ta < plant-(Tave-Tj-2*Ta) <= Ta + Tj:
+                    # 恒速阶段
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                elif Ta + Tj < plant-(Tave-Tj-2*Ta) <= 2 * Ta + Tj:
+                    # 减速阶段
+                    force_vel -= self.force_acc_max * time_step
+                    if force_vel < 0:
+                        force_vel = 0
+                    last_wrench += dir * force_vel * time_step
+                    segment_wrenchs[2] = last_wrench
+                else:
+                    # 超过所有阶段后，使用结束力矩
+                    segment_wrenchs = segment_wrenchs
+                
+                all_wrenchs[count] = copy.deepcopy(segment_wrenchs) 
+                all_wrenchs[count][2] = (all_wrenchs[count][2] - t_start[2]) * coeff + t_start[2]
+                start_wrench = copy.deepcopy(all_wrenchs[count])
+
+        
+        all_wrenchs = np.clip(all_wrenchs, np.array([0, 0, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0]))
+
+        return all_wrenchs
+
+
+FP = ForcePlanner()  
+
+# Example input for testing
+start_wrench = [0, 0, 0, 0, 0, 0]  # Starting wrench
+#end_wrench = [0, 0, 47, 0, 0, 0]
+# end_wrench = [[0, 0, -29, 0, 0, 0]]  # Ending wrench
+end_wrench = [[0, 0, -20, 0, 0, 0], [0, 0, -5, 0, 0, 0], [0, 0, -35, 0, 0, 0],[0, 0, -60, 0, 0, 0], [0, 0, 0, 0, 0, 0]]  # Ending wrench
+
+time_points = [0, 1]  # From time 0 to 1
+#time_points = None
+time_step = 0.1  # 0.1 second steps
+
+# Call the function with example values
+result = FP.S_shaped_wrench_interpolate(start=start_wrench, wrench=end_wrench, time_points=time_points, time_step=0.0083333)
+
+# z_values = result[:, 2]
+# # for t, z in zip(times, z_values):
+# #     print(f"time = {t:.4f} s,\tZ = {z:.4f}")
+
+# plt.figure(figsize=(6, 4))
+# plt.plot(times, z_values, label='Z (third component)')
+# plt.xlabel('Time (s)')
+# plt.ylabel('Force Z')
+# plt.title('S-shaped 插值 —— 第三分量 (Z)')
+# plt.grid(True)
+# plt.legend()
+# plt.tight_layout()
+# plt.show()
+
+
+
+

Massage/MassageControl/algorithms/hybridAdmit_controller.py

@@ -0,0 +1,150 @@
+import sys
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from .arm_state import ArmState
+from .base_controller import BaseController
+from pathlib import Path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+from tools.yaml_operator import read_yaml
+import time
+
+class HybridAdmitController(BaseController):
+    def __init__(self, name, state: ArmState,config_path) -> None:
+        super().__init__(name, state)
+        self.load_config(config_path)
+        self.laset_print_time = 0
+    
+
+    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']}")
+        # 姿态调节器
+        # 位控 2x2矩阵
+        self.Kp_R = np.diag(np.array(config_dict['Kp_R']))
+        self.Ki_R = np.diag(np.array(config_dict['Ki_R']))
+        self.Kd_R = np.diag(np.array(config_dict['Kd_R']))
+
+        mass_rot = np.array(config_dict['mass_z'])
+        stiff_rot = np.array(config_dict['stiff_z'])
+        desired_xi = np.array(config_dict['desired_xi'])
+        damp_rot = np.array(config_dict['damp_z'])
+        self.D_z_min = 10.0
+        self.D_z_max = 100.0
+        self.D_z_lambda = 5.0
+        self.M_z = np.diag(mass_rot)
+        self.K_z = np.diag(stiff_rot)
+        self.D_z = np.diag(damp_rot)
+        # 位控 2x2矩阵
+        self.Kp = np.diag(np.array(config_dict['Kp']))
+        self.Ki = np.diag(np.array(config_dict['Ki']))
+        self.Kd = np.diag(np.array(config_dict['Kd']))
+
+        self.pose_integral_error = np.zeros(6)
+
+    def step(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        temp_pose_error = self.state.arm_position - self.state.desired_position
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+        # 姿态误差(四元数)
+        rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+        # z方向力导纳控制
+        self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2])
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+    def step_traj(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        
+        temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+
+        # 姿态误差(四元数)
+        angular_velocity = np.array(self.state.desired_twist[3:])  # 形状 (3,)
+
+        # 用旋转向量（小角度近似）
+        rotvec = angular_velocity * dt  # 旋转向量 = 角速度 × 时间
+        rot_quat = R.from_rotvec(rotvec).as_quat()  # 转成四元数，形状 (4,)
+        rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+
+        # z方向力导纳控制
+        self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2] + self.state.desired_acc[2]*dt) - self.K_z * self.state.pose_error[2]) + self.state.desired_acc[2]
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+        
+
+
+
+
+
+
+
+
+
+
+
+
+        

Massage/MassageControl/algorithms/hybridPid_controller.py

@@ -0,0 +1,161 @@
+import sys
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from .arm_state import ArmState
+from .base_controller import BaseController
+from pathlib import Path
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+from tools.yaml_operator import read_yaml
+import time
+
+class HybridPidController(BaseController):
+    def __init__(self, name, state: ArmState,config_path) -> None:
+        super().__init__(name, state)
+        self.load_config(config_path)
+        self.laset_print_time = 0
+    
+
+    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']}")
+        # 姿态调节器
+        # 位控 2x2矩阵
+        self.Kp_R = np.diag(np.array(config_dict['Kp_R']))
+        self.Ki_R = np.diag(np.array(config_dict['Ki_R']))
+        self.Kd_R = np.diag(np.array(config_dict['Kd_R']))
+
+        # 力控
+        self.force_mass = np.array(config_dict['force_mass'])
+        self.force_damp = np.array(config_dict['force_damp'])
+        self.Kp_force = np.array(config_dict['Kp_force'])
+        self.Kd_force = np.array(config_dict['Kd_force'])
+        self.Ki_force = np.array(config_dict['Ki_force'])
+        # TODO 修改控制率
+        self.e_t = 0
+        self.e_t1 = 0
+        self.e_t2 = 0
+        self.force_control_value = 0
+
+        # 位控 2x2矩阵
+        self.Kp = np.diag(np.array(config_dict['Kp']))
+        self.Ki = np.diag(np.array(config_dict['Ki']))
+        self.Kd = np.diag(np.array(config_dict['Kd']))
+        self.pose_integral_error = np.zeros(6)
+    
+    def step(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        temp_pose_error = self.state.arm_position - self.state.desired_position
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+        # 姿态误差(四元数)
+        rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+
+        # z方向力控制
+        self.e_t2 = self.e_t1
+        self.e_t1 = self.e_t
+        self.e_t = wrench_err[2]
+        self.force_control_value += self.Kp_force * (self.e_t - self.e_t1) + self.Ki_force * self.e_t * dt + self.Kd_force * (self.e_t - 2 * self.e_t1 + self.e_t2) /dt
+        self.state.arm_desired_acc[2] = (1.0 / self.force_mass) * (self.force_control_value - self.force_damp * self.state.arm_desired_twist[2])
+
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+    def step_traj(self,dt):
+        # 方向统一
+        if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
+            self.state.arm_orientation = -self.state.arm_orientation
+        # 缓存常用计算
+        arm_ori_quat = R.from_quat(self.state.arm_orientation)
+        arm_ori_mat = arm_ori_quat.as_matrix()
+        # 位置误差
+        
+        temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt
+        self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+
+        # 姿态误差(四元数)
+        angular_velocity = np.array(self.state.desired_twist[3:])  # 形状 (3,)
+
+        # 用旋转向量（小角度近似）
+        rotvec = angular_velocity * dt  # 旋转向量 = 角速度 × 时间
+        rot_quat = R.from_rotvec(rotvec).as_quat()  # 转成四元数，形状 (4,)
+        rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+
+        # 期望加速度
+        wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
+
+        # z方向力控制
+        self.e_t2 = self.e_t1
+        self.e_t1 = self.e_t
+        self.e_t = wrench_err[2]
+        self.force_control_value += self.Kp_force * (self.e_t - self.e_t1) + self.Ki_force * self.e_t * dt + self.Kd_force * (self.e_t - 2 * self.e_t1 + self.e_t2) /dt
+        self.state.arm_desired_acc[2] = (1.0 / self.force_mass) * (self.force_control_value - self.force_damp * self.state.arm_desired_twist[2])
+        self.pose_integral_error +=  self.state.pose_error * dt
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]
+        # 姿态pid
+        self.state.arm_desired_acc[3:] = -self.Kd_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+
+        self.clip_command(self.state.arm_desired_acc, "acc")
+        ## 更新速度和位姿
+        self.state.arm_desired_twist += self.state.arm_desired_acc * dt
+        self.clip_command(self.state.arm_desired_twist, "vel")
+        # 计算位姿变化
+        delta_pose = np.concatenate([
+            arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
+            self.state.arm_desired_twist[3:] * dt
+        ])
+        self.clip_command(delta_pose, "pose")
+        # 更新四元数
+        delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
+        arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
+        self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
+        # 归一化四元数
+        self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
+        # 更新位置
+        self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+