From b172aef382ff0f1992353112862f1ab52442ab2b Mon Sep 17 00:00:00 2001
From: swayneleong <15723182159@163.com>
Date: Tue, 27 May 2025 20:35:27 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BB=BF=E7=85=A7=E5=8A=A0=E5=85=A5=E5=85=B6?=
 =?UTF-8?q?=E4=BB=96=E6=8E=A7=E5=88=B6=E5=99=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../algorithms/admithybrid_controller.py      | 135 +++
 .../algorithms/admittanceZ_controller.py      | 153 ++++
 .../algorithms/force_planner.py               | 273 ++++++
 .../algorithms/hybridAdmit_controller.py      | 150 ++++
 .../algorithms/hybridPid_controller.py        | 161 ++++
 .../algorithms/hybrid_controller.py           | 150 ++++
 .../algorithms/interpolation.py               | 817 ++++++++++++++++++
 .../algorithms/positionerSensor_controller.py | 161 ++++
 .../MassageControl/config/admithybrid.yaml    |  13 +
 .../MassageControl/config/admittanceZ.yaml    |  17 +
 Massage/MassageControl/config/hybrid.yaml     |  17 +
 .../MassageControl/config/hybridAdmit.yaml    |  23 +
 Massage/MassageControl/config/hybridPid.yaml  |  22 +
 .../config/positionerSensor.yaml              |  18 +
 14 files changed, 2110 insertions(+)
 create mode 100755 Massage/MassageControl/algorithms/admithybrid_controller.py
 create mode 100755 Massage/MassageControl/algorithms/admittanceZ_controller.py
 create mode 100644 Massage/MassageControl/algorithms/force_planner.py
 create mode 100755 Massage/MassageControl/algorithms/hybridAdmit_controller.py
 create mode 100755 Massage/MassageControl/algorithms/hybridPid_controller.py
 create mode 100755 Massage/MassageControl/algorithms/hybrid_controller.py
 create mode 100755 Massage/MassageControl/algorithms/interpolation.py
 create mode 100755 Massage/MassageControl/algorithms/positionerSensor_controller.py
 create mode 100755 Massage/MassageControl/config/admithybrid.yaml
 create mode 100755 Massage/MassageControl/config/admittanceZ.yaml
 create mode 100755 Massage/MassageControl/config/hybrid.yaml
 create mode 100755 Massage/MassageControl/config/hybridAdmit.yaml
 create mode 100755 Massage/MassageControl/config/hybridPid.yaml
 create mode 100755 Massage/MassageControl/config/positionerSensor.yaml

diff --git a/Massage/MassageControl/algorithms/admithybrid_controller.py b/Massage/MassageControl/algorithms/admithybrid_controller.py
new file mode 100755
index 0000000..7464db1
--- /dev/null
+++ b/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]
diff --git a/Massage/MassageControl/algorithms/admittanceZ_controller.py b/Massage/MassageControl/algorithms/admittanceZ_controller.py
new file mode 100755
index 0000000..522c4ac
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/force_planner.py b/Massage/MassageControl/algorithms/force_planner.py
new file mode 100644
index 0000000..e4ab769
--- /dev/null
+++ b/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()
+
+
+
+
diff --git a/Massage/MassageControl/algorithms/hybridAdmit_controller.py b/Massage/MassageControl/algorithms/hybridAdmit_controller.py
new file mode 100755
index 0000000..b9c0ad3
--- /dev/null
+++ b/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]
+
+        
+
+
+
+
+
+
+
+
+
+
+
+
+        
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/hybridPid_controller.py b/Massage/MassageControl/algorithms/hybridPid_controller.py
new file mode 100755
index 0000000..46d2c60
--- /dev/null
+++ b/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]
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+        
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/hybrid_controller.py b/Massage/MassageControl/algorithms/hybrid_controller.py
new file mode 100755
index 0000000..07b6782
--- /dev/null
+++ b/Massage/MassageControl/algorithms/hybrid_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 HybridController(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']}")
+        # 姿态调节器
+        mass_rot = np.array(config_dict['mass_rot'])
+        stiff_rot = np.array(config_dict['stiff_rot'])
+        desired_xi = np.array(config_dict['desired_xi'])
+        damp_rot = np.array(config_dict['damp_rot'])
+
+        mass_z = np.array(config_dict['mass_z'])
+        stiff_z = np.array(config_dict['stiff_z'])
+        damp_z = np.array(config_dict['damp_z'])
+        for i in range(3):
+            if damp_rot[i] < 0:
+                damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i])
+        self.M_rot = np.diag(mass_rot)
+        self.M_rot_inv = np.linalg.inv(self.M_rot)
+        self.K_rot = np.diag(stiff_rot)
+        self.D_rot = 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)
+		
+        self.M_z = np.diag(mass_z)
+        self.K_z = np.diag(stiff_z)
+        self.D_z = np.diag(damp_z)
+
+
+    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
+        # 姿态控制 期望目标点坐标系的姿态下的力矩为0
+        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:])
+        # 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]
+        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
+        # 姿态控制 期望目标点坐标系的姿态下的力矩为0
+        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:]
+        # 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]
+
+        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]
+
+
+
+
+
+
+
+
+
+
+        
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/interpolation.py b/Massage/MassageControl/algorithms/interpolation.py
new file mode 100755
index 0000000..f1ba2fc
--- /dev/null
+++ b/Massage/MassageControl/algorithms/interpolation.py
@@ -0,0 +1,817 @@
+import numpy as np
+from scipy.interpolate import CubicSpline
+from scipy.spatial.transform import Rotation as R, Slerp
+from scipy.interpolate import BSpline
+from scipy.interpolate import make_interp_spline
+from scipy.signal import savgol_filter
+from scipy.interpolate import interp1d
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import math
+import copy
+
+def linear_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1):
+    """
+    进行位置和/或姿态的线性插值，支持多个点
+    :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选)
+    :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选)
+    :param time_points: 时间点序列 (N个浮点数)
+    :param time_step: 离散时间步长，单位为秒 (float)
+    :return: 插值序列，包含位置序列、姿态序列或两者兼有
+    """
+    if positions is None and quaternions 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 positions is not None:
+        all_positions = np.zeros((len(times), positions.shape[1]))
+        segment_durations = np.diff(time_points)
+        segment_counts = np.floor(segment_durations / time_step).astype(int)
+        current_idx = 0
+        for i in range(len(segment_counts)):
+            segment_times = np.linspace(0, segment_durations[i], segment_counts[i] + 1)
+            if i > 0:
+                segment_times = segment_times[1:]
+            start_pos = positions[i]
+            end_pos = positions[i + 1]
+            segment_positions = start_pos + np.outer(segment_times, (end_pos - start_pos) / segment_durations[i])
+            all_positions[current_idx:current_idx + len(segment_positions)] = segment_positions
+            current_idx += len(segment_positions)
+
+         # 确保最后一个位置被处理
+        if current_idx < len(times):
+            all_positions[current_idx:] = positions[-1]
+
+    if quaternions is not None:
+        slerp = Slerp(time_points, R.from_quat(quaternions))
+        all_quaternions = slerp(times).as_quat()
+    
+        all_quaternions = np.array([quat / np.linalg.norm(quat) for quat in all_quaternions])
+
+    if positions is not None and quaternions is not None:
+        return all_positions, all_quaternions
+    elif positions is not None:
+        return all_positions
+    elif quaternions is not None:
+        return all_quaternions
+
+def spline_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1):
+    """
+    进行位置和/或姿态的样条插值，支持多个点
+    :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选)
+    :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选)
+    :param time_points: 时间点序列 (N个浮点数)
+    :param time_step: 离散时间步长，单位为秒 (float)
+    :return: 插值序列，包含位置序列、姿态序列或两者兼有
+    """
+    if positions is None and quaternions is None:
+        raise ValueError("至少需要输入位置或姿态的序列")
+    if time_points is None or len(time_points) < 2:
+        raise ValueError("需要提供至少两个时间点")
+
+    all_positions = []
+    all_quaternions = []
+    times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step)
+
+    if positions is not None:
+        cs = CubicSpline(time_points, positions, axis=0)
+        all_positions = cs(times)
+
+    if quaternions is not None:
+        slerp = Slerp(time_points, R.from_quat(quaternions))
+        all_quaternions = slerp(times).as_quat()
+
+    if positions is not None and quaternions is not None:
+        return np.array(all_positions), np.array(all_quaternions)
+    elif positions is not None:
+        return np.array(all_positions)
+    elif quaternions is not None:
+        return np.array(all_quaternions)
+
+
+
+def generate_circle_trajectory(center, omega=0.4, radius=0.02, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
+    """
+    Generate a 3D trajectory of a circular motion from the center to the specified radius.
+
+    Parameters:
+    center (list): The center of the circle [x, y, z , r , p , y].
+    omega (float): The angular velocity.
+    radius (float): The radius of the circle.
+    reverse (bool): If True, rotates counterclockwise. If False, rotates clockwise.
+    time_points (list or np.ndarray): List or array of time points.
+    time_step (float): Time step for generating the trajectory.
+    start_transition_duration (float): Duration for the transition at the start.
+    end_transition_duration (float): Duration for the transition at the end.
+
+    Returns:
+    np.ndarray: Array of positions over time.
+    """
+
+    # print(time_points)
+    if time_points is None or len(time_points) < 2:
+        raise ValueError("需要提供至少两个时间点")
+    
+    
+    if start_transition_duration is None:
+        start_transition_duration = 2
+
+    if end_transition_duration is None:
+        end_transition_duration = 2
+    t_points = time_points.copy()
+    t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration
+    t_points[-1] = round(t_points[-1] / time_step) * time_step
+    times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step)
+
+    if reverse:
+        angles = -omega * times
+    else:
+        angles = omega * times
+
+    radii = np.ones_like(times) * radius
+
+    start_transition = times < start_transition_duration
+    end_transition = times > (times[-1] - end_transition_duration)
+    radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2
+    radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2
+    
+    x_positions = center[0][0] + radii * np.cos(angles)
+    y_positions = center[0][1] + radii * np.sin(angles)
+    z_positions = np.full_like(x_positions, center[0][2])  # Z position remains constant
+
+    positions = np.column_stack((x_positions, y_positions, z_positions))
+    return positions
+
+def bezier_curve(control_points, n_points=100):
+    # 确保输入为二维 float64 数组
+    control_points = np.array(control_points, dtype=np.float64)
+    if control_points.ndim != 2:
+        raise ValueError("control_points must be a 2D array, got shape {}".format(control_points.shape))
+
+    n = len(control_points) - 1
+    t = np.linspace(0.0, 1.0, n_points)
+    curve = np.zeros((n_points, control_points.shape[1]), dtype=np.float64)
+
+    def bernstein(i, n, t):
+        from scipy.special import comb
+        return comb(n, i) * ((1 - t) ** (n - i)) * (t ** i)
+
+    for i in range(n + 1):
+        # 明确每个控制点是 float64 向量
+        point = np.asarray(control_points[i], dtype=np.float64)
+        if point.shape != (control_points.shape[1],):
+            raise ValueError(f"Invalid point shape: {point.shape}")
+        b = bernstein(i, n, t)
+        curve += np.outer(b, point)
+
+    return curve
+
+
+
+def cloud_point_interpolate(positions=None, quaternions=None, time_points=None, time_step=0.1):
+    """
+    进行输入的点云位置和/或姿态的曲线插值，支持多个点，采用B样条插值法
+    :param positions: 位置序列，单位为米 (Nx2 或 Nx3 numpy array, 可选)
+    :param quaternions: 姿态序列，四元数 (Nx4 numpy array, 可选)
+    :param time_points: 时间点序列 (N个浮点数)
+    :param time_step: 离散时间步长，单位为秒 (float)
+    :return: 插值序列，包含位置序列、姿态序列或两者兼有
+    """
+    if positions is None and quaternions is None:
+        raise ValueError("至少需要输入位置或姿态的序列")
+    if time_points is None or len(time_points) < 2:
+        raise ValueError("需要提供至少两个时间点")
+
+    temp_positions = np.array(positions)
+    temp_quaternions = np.zeros((len(quaternions), 4))
+
+    # 将RPY角度转换为四元数
+    for i in range(len(quaternions)):
+        temp_quaternions[i] = R.from_euler('xyz', quaternions[i]).as_quat()
+
+    time_points = np.linspace(time_points[0], time_points[-1], len(temp_positions))
+    times = np.arange(time_points[0], time_points[-1] + time_step * 0.9, time_step)
+
+    temp_positions_smoothed = copy.deepcopy(temp_positions)
+    # temp_positions = np.array(positions, dtype=np.float64)
+    # temp_positions_smoothed = bezier_curve(temp_positions, n_points=len(temp_positions))
+    temp_quaternions_smoothed = copy.deepcopy(temp_quaternions)
+
+    all_positions, all_quaternions = [], []
+
+    # 进行B样条插值
+    if temp_positions_smoothed is not None:
+        BS = make_interp_spline(time_points, temp_positions_smoothed, k=3, bc_type='natural')
+        all_positions = BS(times)
+
+    # 进行四元数LERP插值
+    if temp_quaternions_smoothed is not None:
+        temp_quaternions_smoothed = np.array(temp_quaternions_smoothed)
+        all_quaternions = []
+        for t in times:
+            # 在时间点序列中找到最近的两个时间点，用于线性插值
+            idx = np.searchsorted(time_points, t) - 1
+            idx = max(0, min(idx, len(time_points) - 2))
+            
+            t1, t2 = time_points[idx], time_points[idx + 1]
+            q1, q2 = temp_quaternions_smoothed[idx], temp_quaternions_smoothed[idx + 1]
+            
+            # 计算插值因子 alpha
+            alpha = (t - t1) / (t2 - t1)
+            
+            # 进行四元数LERP插值
+            q_interp = (1 - alpha) * q1 + alpha * q2
+            q_interp /= np.linalg.norm(q_interp)  # 归一化四元数
+            
+            all_quaternions.append(q_interp)
+
+    euler_angles = R.from_quat(all_quaternions).as_euler('xyz')
+    
+    # 检查数据点数量
+    if len(euler_angles) >= 5:
+        # 数据点数量大于等于3时使用 Savitzky-Golay 滤波器
+        euler_angles_smoothed = savgol_filter(euler_angles, window_length=5, polyorder=3, axis=0)
+    else:
+        # 数据点小于5时直接使用原始数据
+        euler_angles_smoothed = euler_angles
+
+    # 将平滑后的欧拉角转换回四元数
+    all_quaternions = R.from_euler('xyz', euler_angles_smoothed).as_quat()
+
+    if temp_positions is not None and temp_quaternions is not None:
+        return np.array(all_positions), np.array(all_quaternions)
+    elif temp_positions is not None:
+        return np.array(all_positions)
+    elif temp_quaternions is not None:
+        return np.array(all_quaternions)
+    
+def oscillation_wrench_interpolate(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 linear_wrench_interpolate(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, -70, 0, 0, 0]), np.array([0, 0, 0, 0, 0, 0]))
+
+    if wrench is not None:
+        return np.array(all_wrenchs)
+    
+def resample_curve_strict(points, num_resampled_points):
+    """
+    修正的曲线重新采样函数，移除重复点并确保累积弧长严格递增。支持三维曲线。
+    :param points: Nx3 numpy array，三维轨迹点
+    :param num_resampled_points: 重新采样的点数
+    :return: Nx3 numpy array，重采样后的三维轨迹
+    """
+    points = np.array(points, dtype=np.float64)
+
+    # 计算累积弧长
+    distances = np.linalg.norm(np.diff(points, axis=0), axis=1)
+    cumulative_length = np.insert(np.cumsum(distances), 0, 0)
+
+    # 移除重复点（累积弧长未变化的）
+    unique_indices = np.where(np.diff(cumulative_length, prepend=-np.inf) > 0)[0]
+    cumulative_length = cumulative_length[unique_indices]
+    points = points[unique_indices]
+
+    # 生成等间隔弧长采样点
+    target_lengths = np.linspace(0, cumulative_length[-1], num_resampled_points)
+
+    # 对每个维度做线性插值
+    interp_funcs = [interp1d(cumulative_length, points[:, i], kind='linear', fill_value="extrapolate") for i in range(3)]
+    new_coords = [interp(target_lengths) for interp in interp_funcs]
+
+    return np.column_stack(new_coords)
+
+
+
+def circle_trajectory(center, omega=8.0, radius=0.04, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
+
+
+    if time_points is None or len(time_points) < 2:
+        raise ValueError("需要提供至少两个时间点")
+    
+    
+    if start_transition_duration is None:
+        start_transition_duration = 2
+
+    if end_transition_duration is None:
+        end_transition_duration = 2
+    t_points = time_points.copy()
+    # t_points[-1] = time_points[-1] + end_transition_duration + start_transition_duration
+    t_points[-1] = time_points[-1]
+    t_points[-1] = round(t_points[-1] / time_step) * time_step
+    times = np.arange(t_points[0], t_points[-1] + time_step * 0.9, time_step)
+
+    if reverse:
+        angles = -omega * times
+    else:
+        angles = omega * times
+
+    radii = np.ones_like(times) * radius
+
+    start_transition = times < start_transition_duration
+    end_transition = times > (times[-1] - end_transition_duration)
+    radii[start_transition] = radius * (1 - np.cos(np.pi * times[start_transition] / start_transition_duration)) / 2
+    radii[end_transition] = radius * (1 + np.cos(np.pi * (times[end_transition] - (times[-1] - end_transition_duration)) / end_transition_duration)) / 2
+    
+    x_positions = radii * np.cos(angles)
+    y_positions = radii * np.sin(angles)
+    z_positions = np.full_like(x_positions, 0)  # Z position remains constant
+
+
+
+    positions = np.column_stack((x_positions, y_positions, z_positions))
+    positions = resample_curve_strict(positions, len(positions))
+    # print("circle_positions:")
+    tempToolRPY = R.from_euler('xyz', center[0][3:], degrees=False).as_matrix()
+    for i in range(len(positions)):
+        positions[i] = tempToolRPY @  positions[i] + center[0][:3]       # 将RPY角度转换为四元数
+        # print(positions[i])
+        
+    return positions
+
+# def generate_circle_cloud_points(center, start_point, radius, delta_theta = 10*np.pi/180, num_turns = 3):
+#     """
+#     center: 圆心坐标，形如 (x_c, y_c)
+#     start_point: 起始点坐标，形如 (x_0, y_0)
+#     radius: 圆的半径
+#     delta_theta: 每次插补的角度增量
+#     num_turns: 绕圈的次数
+#     """
+#     # 确定总共需要生成的插补点数
+#     num_points = int((2 * np.pi * num_turns) / delta_theta)
+    
+#     # 圆心
+#     x_c, y_c = center
+    
+#     # 计算起始点的初始角度
+#     x_0, y_0 = start_point
+#     theta_0 = np.arctan2(y_0 - y_c, x_0 - x_c)
+    
+#     # 初始化存储插补点的列表
+#     circle_points = []
+    
+#     # 生成插补点
+#     for i in range(num_points):
+#         # 当前角度
+#         theta_i = theta_0 + i * delta_theta
+        
+#         # 计算插补点的坐标
+#         x_i = x_c + radius * np.cos(theta_i)
+#         y_i = y_c + radius * np.sin(theta_i)
+        
+#         # 将点添加到列表中
+
+#         circle_points.append((np.round(x_i).astype(int), np.round(y_i).astype(int)))
+
+#     circle_points.append((np.round(x_0).astype(int), np.round(y_0).astype(int)))
+    
+#     return circle_points
+
+    
+
+def calculate_target_Euler(point):
+
+    temp_euler = np.zeros((len(point), 3), dtype=np.float64)
+
+    for i in range(len(point)):
+        if(point[i][5]<0): 
+            temp_euler[i][0] = -math.asin(-point[i][4])
+            temp_euler[i][1] =  math.atan2(-point[i][3],-point[i][5])
+        else:
+            temp_euler[i][0] = -math.asin(point[i][4])
+            temp_euler[i][1] =  math.atan2(point[i][3],point[i][5])
+                
+        temp_euler[i][2] =  0.0
+
+    return temp_euler
+
+
+if __name__ == "__main__":  
+    # import pathlib
+    # import sys
+    # sys.path.append(str(pathlib.Path.cwd()))
+    # from MassageControl.tools.draw_tools import plot_trajectory
+
+    import numpy as np
+    import pandas as pd
+    import matplotlib.pyplot as plt
+
+    # 示例使用
+    center = (80, 90)  # 圆心
+    start_point = (70, 0)  # 起点
+    radius =  np.linalg.norm(np.array(start_point) - np.array(center))  # 半径
+    delta_theta = np.pi / 9  # 每次插补的角度增量
+    num_turns = 2  # 绕2圈
+
+    # 生成圆的插补点
+    circle_points_with_start = generate_circle_cloud_points(center, start_point, radius, delta_theta, num_turns)
+
+    # 将生成的插补点转换为可视化的DataFrame
+    circle_points_with_start_df = pd.DataFrame({
+        "x": [point[0] for point in circle_points_with_start],
+        "y": [point[1] for point in circle_points_with_start]
+    })
+
+    # 打印生成的插补点
+    print(circle_points_with_start_df)
+
+    # 绘制插补点的图像
+    x_vals = [point[0] for point in circle_points_with_start]
+    y_vals = [point[1] for point in circle_points_with_start]
+
+    plt.figure(figsize=(6, 6))
+    plt.scatter(x_vals, y_vals, marker='o', linestyle='-', color='b')
+    plt.scatter([x_vals[0]], [y_vals[0]], color='r', label='Start Point')  # 标记起点
+    plt.gca().set_aspect('equal', adjustable='box')
+    plt.title('Circle Interpolation Points with Start Point')
+    plt.xlabel('X')
+    plt.ylabel('Y')
+    plt.legend()
+    plt.grid(True)
+
+    # 保存并显示图片
+    plt.show()
+
+
+
+#     start1pos = np.array([0.0392,-0.408,0.752])
+#     end1pos = np.array([0.0392,0.2,0.752])
+
+#     start2pos = np.array([0.2281194148213992,-0.1320499159555817,0.7499999952316284])
+#     end2pos = np.array([0.14268880718774088,-0.13746791895961052,0.7350000095367432])
+
+#     start3pos = np.array([0.23648124242722512,-0.2097320409627573,0.7430000257492065])
+#     end3pos = np.array([0.1493414817211018,-0.21703966731273366,0.7340000224113464])
+
+#     start4pos = np.array([0.24389595888042098,-0.30559190060482105,0.7499999952316284])
+#     end4pos = np.array([0.15822969083491112,-0.3106326911577041,0.7440000128746033])
+
+#     start5pos = np.array([0.2535787008200847,-0.402571052456421,0.7559999775886536])
+#     end5pos = np.array([0.16737854928028986,-0.41016720685793384,0.7580000114440918])
+
+
+    
+
+    
+
+
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(start2pos - end1pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = end1pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (start2pos - end1pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = start2pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(end2pos - start2pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = start2pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (end2pos - start2pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = end2pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(start3pos - end2pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = end2pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (start3pos - end2pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = start3pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(end3pos - start3pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = start3pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (end3pos - start3pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = end3pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(start4pos - end3pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = end3pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (start4pos - end3pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = start4pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(end4pos - start4pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = start4pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (end4pos - start4pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = end4pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(start5pos - end4pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = end4pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (start5pos - end4pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = start5pos
+
+#     # points = np.vstack((points, temppoints))
+
+#     # #--------------------------
+#     # # 计算起始点和结束点之间的总距离
+#     # total_distance = np.linalg.norm(end5pos - start5pos)
+    
+#     # # 根据欧式距离步长为0.005估算所需的点数
+#     # num_points = int(total_distance / dis)
+
+#     # # 初始化用于存储点的数组
+#     # temppoints = np.zeros((num_points + 1, 3))
+#     # temppoints[0] = start5pos
+
+#     # # 生成欧式距离差约为0.01的点
+#     # for i in range(1, num_points + 1):
+#     #     direction = (end5pos - start5pos) / total_distance  # 单位方向向量
+#     #     temppoints[i] = temppoints[i-1] + direction * dis
+
+#     # # 确保最后一个点与endpos完全重合
+#     # temppoints[-1] = end5pos
+
+#     # points = np.vstack((points, temppoints))
+
+
+#     positions_2d = np.vstack([start1pos,end1pos])
+
+
+#     #print(points)
+
+#     time_points = np.array([0,10])
+#     time_step = 0.01
+#     #print(time_points)
+#     #使用点云B样条插值
+#     temppose = np.array([1,1,1,1,0,0])
+#     positions_2d_interp = circle_trajectory(center=temppose,radius=0.05,time_points=time_points,time_step=time_step)
+#     quaternions_interp = np.tile(R.from_euler('xyz', np.array(temppose[3:])).as_quat(), (positions_2d_interp.shape[0], 1))
+#     # positions_2d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step)
+#     # print("2D Position Trajectory (Spline):")
+#     # print(positions_2d)
+    
+#     # # 分解轨迹的x, y, z坐标
+#     x_trajectory, y_trajectory, z_trajectory = zip(*positions_2d_interp)
+
+#     # 分解散点的x, y, z坐标
+#     x_scatter, y_scatter, z_scatter = zip(*positions_2d)
+
+#     # 创建一个3D图形
+#     fig = plt.figure()
+#     ax = fig.add_subplot(111, projection='3d')
+
+#     # # 设置字体为 SimHei (黑体)
+#     # plt.rcParams['font.sans-serif'] = ['SimHei']  # 使用黑体
+#     # plt.rcParams['axes.unicode_minus'] = False    # 解决负号无法正常显示的问题
+
+#     # 绘制3D连续轨迹
+#     ax.plot(x_trajectory, y_trajectory, z_trajectory, label='B-spline interpolation trajectory', color='b')
+
+#     # 绘制3D散点图
+#     ax.scatter(x_scatter, y_scatter, z_scatter, label='cloud points', color='r', marker='o')
+
+#     # 设置轴标签
+#     ax.set_xlabel('X Label')
+#     ax.set_ylabel('Y Label')
+#     ax.set_zlabel('Z Label')
+
+#     # 设置标题
+#     ax.set_title('3D Trajectory and Scatter Plot')
+
+#     # 添加图例
+#     ax.legend()
+
+#     # 设置轴的比例为相等
+#     plt.gca().set_aspect('equal', adjustable='box')
+#     # # 显示图形
+#     # plt.show()
+
+#     plot_trajectory(positions_2d_interp, quaternions_interp)
+
+#     # for i in range(10):
+#     #     tempX = np.linspace(startpos[0], endpos[0], 10)
+
+#    # 示例使用
+#     positions_2d = np.array([[0, 0], [1, 1], [1, 2],[2,2]]) #, [1, 1], [2, 1], [1, 1], [1, 1],[0, 0]])
+#     positions_3d = np.array([[0, 0, 0], [1, 1, 1], [2, 0, 2], [3, 1, 3]])
+#     quaternions = np.array([[0, 0, 0, 1], [0.707, 0, 0, 0.707], [1, 0, 0, 0], [1, 0, 0, 0]])
+#     time_points = np.array([0, 1, 3, 4])
+#     time_step = 0.5
+
+#     # # 只插值2D位置
+#     # positions_2d_interp = linear_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step)
+#     # print("2D Position Trajectory:")
+#     # print(positions_2d_interp)
+
+#     # # 只插值3D位置
+#     # positions_3d_interp = linear_interpolate(positions=positions_3d, time_points=time_points, time_step=time_step)
+#     # print("3D Position Trajectory:")
+#     # print(positions_3d_interp)
+
+#     # # 只插值姿态
+#     # quaternions_interp = linear_interpolate(quaternions=quaternions, time_points=time_points, time_step=time_step)
+#     # print("Quaternion Trajectory:")
+#     # print(quaternions_interp)
+
+#     # # 同时插值3D位置和姿态
+#     # positions_3d_interp, quaternions_interp = linear_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step)
+#     # print("3D Position and Quaternion Trajectory:")
+#     # print(positions_3d_interp)
+#     # print(quaternions_interp)
+
+#     # # 绘制插值轨迹
+#     # plot_trajectory(positions_2d_interp)
+#     # plot_trajectory(positions_3d_interp)
+#     # plot_trajectory(quaternions_interp)
+#     # plot_trajectory(positions_3d_interp, quaternions_interp)
+
+#     # # 使用样条插值
+#     # positions_2d_interp = spline_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step)
+#     # print("2D Position Trajectory (Spline):")
+#     # print(positions_2d_interp)
+#     # plot_trajectory(positions_2d_interp)
+
+#     # positions_3d_interp, quaternions_interp = spline_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step)
+#     # print("3D Position and Quaternion Trajectory (Spline):")
+#     # print(positions_3d_interp)
+#     # print(quaternions_interp)
+#     # plot_trajectory(positions_3d_interp, quaternions_interp)
+
+#     # # 使用点云B样条插值
+#     # positions_2d_interp = cloud_point_interpolate(positions=positions_2d, time_points=time_points, time_step=time_step)
+#     # print("2D Position Trajectory (Spline):")
+#     # print(positions_2d_interp)
+#     # plot_trajectory(positions_2d_interp)
+
+#     # positions_3d_interp, quaternions_interp = cloud_point_interpolate(positions=positions_3d, quaternions=quaternions, time_points=time_points, time_step=time_step)
+#     # print("3D Position and Quaternion Trajectory (Spline):")
+#     # print(positions_3d_interp)
+#     # print(quaternions_interp)
+#     # plot_trajectory(positions_3d_interp, quaternions_interp)
\ No newline at end of file
diff --git a/Massage/MassageControl/algorithms/positionerSensor_controller.py b/Massage/MassageControl/algorithms/positionerSensor_controller.py
new file mode 100755
index 0000000..215329d
--- /dev/null
+++ b/Massage/MassageControl/algorithms/positionerSensor_controller.py
@@ -0,0 +1,161 @@
+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
+import time
+sys.path.append(str(Path(__file__).resolve().parent.parent))
+
+from tools.yaml_operator import read_yaml
+# from pykalman import KalmanFilter
+import copy
+
+class PositionerSensorController(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']}")
+        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_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']))
+
+        # 姿态 3x3矩阵
+        self.Kp_M = np.diag(np.array(config_dict['Kp_M']))
+        self.Ki_M = np.diag(np.array(config_dict['Ki_M']))
+        self.Kd_M = np.diag(np.array(config_dict['Kd_M']))
+
+        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(1):
+            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.K_tran = np.diag(stiff_tran)
+        self.D_tran = np.diag(damp_tran)
+
+        self.laset_print_time = 0
+
+    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_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd_M @ self.state.arm_desired_twist[:2] - self.Kp_M @ self.state.pose_error[:2] - self.Ki_M @ self.pose_integral_error[:2]
+        self.state.arm_desired_acc[2] = (1/self.M_tran) * (wrench_err[2] - self.D_tran * (self.state.arm_desired_twist[2] - self.state.desired_twist[2])-self.K_tran * self.state.pose_error[2])
+
+        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_R @ self.state.arm_desired_twist[3:] - self.Kp_R @ self.state.pose_error[3:] - self.Ki_R @ self.pose_integral_error[3:]
+        # 位控制
+        self.state.arm_desired_acc[:2] = -self.Kd_M @ self.state.arm_desired_twist[:2] - self.Kp_M @ self.state.pose_error[:2] - self.Ki_M @ self.pose_integral_error[:2]
+        self.state.arm_desired_acc[2] = (1/self.M_tran) * (wrench_err[2] - self.D_tran * (self.state.arm_desired_twist[2] - self.state.desired_twist[2] + self.state.desired_acc[2]*dt)-self.K_tran * self.state.pose_error[2]) + self.state.desired_acc[2]
+
+
+        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 = PositionerSensorController("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)
+
diff --git a/Massage/MassageControl/config/admithybrid.yaml b/Massage/MassageControl/config/admithybrid.yaml
new file mode 100755
index 0000000..9efbfab
--- /dev/null
+++ b/Massage/MassageControl/config/admithybrid.yaml
@@ -0,0 +1,13 @@
+name: 'admithybrid'
+
+desired_xi: 1
+pos_scale_factor: 1
+mass_tran: [3.0, 3.0, 3.0]
+stiff_tran: [270, 270, 270]
+damp_tran: [30, 30, 30]
+
+
+Kp: [1,1,1]
+Ki: [0, 0, 0]
+Kd: [1, 1, 1]
+
diff --git a/Massage/MassageControl/config/admittanceZ.yaml b/Massage/MassageControl/config/admittanceZ.yaml
new file mode 100755
index 0000000..5a74508
--- /dev/null
+++ b/Massage/MassageControl/config/admittanceZ.yaml
@@ -0,0 +1,17 @@
+name: admittanceZ
+
+
+desired_xi: 1
+pos_scale_factor: 1
+rot_scale_factor: 1
+
+mass_tran: [3.0, 3.0, 3.0]
+mass_rot: [0.11, 0.11, 0.11]
+stiff_tran: [270, 270, 0]
+stiff_rot: [11, 11, 11]
+damp_tran: [30, 30, 30]
+damp_rot: [1.1, 1.1, 1.1]
+
+
+
+
diff --git a/Massage/MassageControl/config/hybrid.yaml b/Massage/MassageControl/config/hybrid.yaml
new file mode 100755
index 0000000..bf95136
--- /dev/null
+++ b/Massage/MassageControl/config/hybrid.yaml
@@ -0,0 +1,17 @@
+name: 'hybrid'
+
+mass_rot: [0.11, 0.11, 0.11]
+stiff_rot: [11, 11, 11]
+damp_rot: [1.1, 1.1, 1.1]
+desired_xi: 1.0
+
+mass_z: [3.0]
+stiff_z: [0]
+damp_z: [30]
+
+# 位控参数
+Kp: [20,20]
+Ki: [0.2,0.2]
+Kd: [10,10]
+
+
diff --git a/Massage/MassageControl/config/hybridAdmit.yaml b/Massage/MassageControl/config/hybridAdmit.yaml
new file mode 100755
index 0000000..e6c1d5d
--- /dev/null
+++ b/Massage/MassageControl/config/hybridAdmit.yaml
@@ -0,0 +1,23 @@
+name: 'hybridAdmit'
+
+mass_z: [3.0]
+stiff_z: [0]
+damp_z: [30]
+
+# 位控参数
+Kp: [20,20]
+Ki: [0.2,0.2]
+Kd: [10,10]
+
+# 位控参数
+Kp_R: [1,1,1]
+Ki_R: [0,0,0]
+Kd_R: [1,1,1]
+
+
+
+
+# # 位控参数
+# Kp: [90,90]
+# Kd: [25,25]
+
diff --git a/Massage/MassageControl/config/hybridPid.yaml b/Massage/MassageControl/config/hybridPid.yaml
new file mode 100755
index 0000000..1c47ad9
--- /dev/null
+++ b/Massage/MassageControl/config/hybridPid.yaml
@@ -0,0 +1,22 @@
+name: 'hybridPid'
+
+
+# 力控参数
+force_mass: 2.5
+force_damp: 25
+Kp_force: 2.0  #0.6
+Kd_force: 0.0
+Ki_force: 0.0  #0
+
+
+# 位控参数
+Kp: [20,20]
+Ki: [0.2,0.2]
+Kd: [10,10]
+
+# 位控参数
+Kp_R: [1,1,1]
+Ki_R: [0,0,0]
+Kd_R: [1,1,1]
+
+
diff --git a/Massage/MassageControl/config/positionerSensor.yaml b/Massage/MassageControl/config/positionerSensor.yaml
new file mode 100755
index 0000000..5b61039
--- /dev/null
+++ b/Massage/MassageControl/config/positionerSensor.yaml
@@ -0,0 +1,18 @@
+name: positionerSensor
+
+desired_xi: 1
+pos_scale_factor: 1
+mass_tran: [3.0]
+stiff_tran: [270]
+damp_tran: [30]
+
+
+Kp_R: [1,1,1]
+Ki_R: [0, 0, 0]
+Kd_R: [1, 1, 1]
+
+# 位控参数
+Kp_M: [20,20]
+Ki_M: [0.2,0.2]
+Kd_M: [10,10]
+