From 3b37aedae241128fa728423b3f3b76f2caa3d3fb Mon Sep 17 00:00:00 2001 From: Swayne Date: Sun, 25 May 2025 20:14:23 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8D=95=E5=91=A8=E6=9C=9F=E5=AF=B9=E5=8F=96?= =?UTF-8?q?=E9=80=86=E8=BF=9B=E8=A1=8C=E7=89=B9=E6=AE=8A=E5=A4=84=E7=90=86?= =?UTF-8?q?=EF=BC=8C=E9=99=8D=E4=BD=8ECPU=E4=BD=BF=E7=94=A8=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5.py | 9 ++++++++- .../MassageControl/algorithms/admittance_controller.py | 4 +++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 46ddd72..335a8db 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -128,7 +128,14 @@ class MassageRobot: # 更新步骤 def kalman_update(self,x_predict, P_predict, z, R): # 卡尔曼增益 - K = P_predict @ np.linalg.inv(P_predict + R) + # K = P_predict @ np.linalg.inv(P_predict + R) + S = P_predict + R + s = np.diag(S) # shape (6,) + p_diag = np.diag(P_predict) + K_diag = np.zeros_like(s) + nonzero_mask = s != 0 + K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask] + K = np.diag(K_diag) # 更新状态 x_update = x_predict + K @ (z - x_predict) # 更新误差协方差 diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py index 6034c33..d88784f 100644 --- a/Massage/MassageControl/algorithms/admittance_controller.py +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -33,6 +33,7 @@ class AdmittanceController(BaseController): if damp_rot[i] < 0: damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i]) self.M = np.diag(np.concatenate([mass_tran, mass_rot])) + self.M_inv = np.linalg.inv(self.M) self.K = np.diag(np.concatenate([stiff_tran, stiff_rot])) self.D = np.diag(np.concatenate([damp_tran, damp_rot])) @@ -59,7 +60,8 @@ class AdmittanceController(BaseController): wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench # if time.time() - self.laset_print_time > 1: # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') - self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) + # self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) + self.state.arm_desired_acc = self.M_inv @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) # if time.time() - self.laset_print_time > 5: # print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) self.clip_command(self.state.arm_desired_acc,"acc")