DevelopmentTeam/MassageRobot_Dobot
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

单周期对取逆进行特殊处理，降低CPU使用率

Browse Source
This commit is contained in:
Swayne 2025-05-25 20:14:23 +08:00
parent 80f0074e9a
commit 3b37aedae2
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -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)
# 更新误差协方差

4
Massage/MassageControl/algorithms/admittance_controller.py
View File

@ -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")