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

优化STEP算法，降低CPU使用率

Browse Source
This commit is contained in:
Ziwei.He 2025-05-26 13:38:58 +08:00
parent 3b37aedae2
commit e1583b1756
3 changed files with 239 additions and 1324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc
View File

Binary file not shown.

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

@ -40,73 +40,84 @@ class AdmittanceController(BaseController):
self.laset_print_time = 0 self.laset_print_time = 0
def step(self,dt): def step(self,dt):
# 计算误差 位置直接作差，姿态误差以旋转向量表示 # 方向统一
temp_pose_error = self.state.arm_position - self.state.desired_position
# if time.time() - self.laset_print_time > 5:
# print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}')
if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
self.state.arm_orientation = -self.state.arm_orientation self.state.arm_orientation = -self.state.arm_orientation
self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error # 缓存常用计算
# if time.time() - self.laset_print_time > 5: arm_ori_quat = R.from_quat(self.state.arm_orientation)
# print("pose_error:",self.state.pose_error[:3]) arm_ori_mat = arm_ori_quat.as_matrix()
# 计算误差 位置直接作差，姿态误差以旋转向量表示 # 位置误差
#rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T temp_pose_error = self.state.arm_position - self.state.desired_position
rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix() self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
# print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}') # 姿态误差(四元数)
rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False) rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
self.state.pose_error[3:] = -rot_err_rotvex self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
# 期望加速度
#wrench_err = self.state.external_wrench_base - self.state.desired_wrench
wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
# if time.time() - self.laset_print_time > 1: KD_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
# print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') KD_pose = 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 - KD_vel - KD_pose)
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) self.clip_command(self.state.arm_desired_acc, "acc")
# 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.state.arm_desired_twist += self.state.arm_desired_acc * dt
self.clip_command(self.state.arm_desired_acc,"acc") self.clip_command(self.state.arm_desired_twist, "vel")
self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist # 计算位姿变化
self.clip_command(self.state.arm_desired_twist,"vel") delta_pose = np.concatenate([
delta_pose = self.state.arm_desired_twist * dt arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
self.state.arm_desired_twist[3:] * dt
# delta_pose[:3] = self.pos_scale_factor * delta_pose[:3] ])
# delta_pose[3:] = self.rot_scale_factor * delta_pose[3:] self.clip_command(delta_pose, "pose")
# if time.time() - self.laset_print_time > 5: # 更新四元数
# print("delta_pose:",delta_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)
delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3] self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
# if time.time() - self.laset_print_time > 5:
# print("tf_delta_pose:",delta_pose)
self.clip_command(delta_pose,"pose")
# testlsy
delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
# random_array = np.random.rand(3) # 生成长度为 3 的数组
# delta_ori_mat = R.from_rotvec(random_array/100000).as_matrix()
arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat
self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
# 归一化四元数 # 归一化四元数
self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) 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] self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
# if time.time() - self.laset_print_time > 2:
# print("-------------admittance_1-------------------------------")
# print("arm_position:",self.state.arm_position) # # 计算误差 位置直接作差，姿态误差以旋转向量表示
# print("desired_position:",self.state.desired_position) # temp_pose_error = self.state.arm_position - self.state.desired_position
# print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') # if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
# print("self.state.arm_desired_acc:",self.state.arm_desired_acc) # self.state.arm_orientation = -self.state.arm_orientation
# print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True)) # self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error
# print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True)) # rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
# print("arm_position_command",self.state.arm_position_command) # rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
# print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True)) # self.state.pose_error[3:] = -rot_err_rotvex
# print("delta_pose:",delta_pose) # wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
# print("delta_ori_mat",delta_ori_mat) # 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)
# self.laset_print_time = time.time() # self.clip_command(self.state.arm_desired_acc,"acc")
# self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
# self.clip_command(self.state.arm_desired_twist,"vel")
# delta_pose = self.state.arm_desired_twist * dt
# delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
# self.clip_command(delta_pose,"pose")
# # testlsy
# delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
# ## 关闭姿态转动
# # random_array = np.random.rand(3) # 生成长度为 3 的数组
# # delta_ori_mat = R.from_rotvec(random_array/100000).as_matrix()
# arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
# arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat
# self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
# # 归一化四元数
# self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
# self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
# # if time.time() - self.laset_print_time > 2:
# # print("-------------admittance_1-------------------------------")
# # print("arm_position:",self.state.arm_position)
# # print("desired_position:",self.state.desired_position)
# # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
# # print("self.state.arm_desired_acc:",self.state.arm_desired_acc)
# # print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True))
# # print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True))
# # print("arm_position_command",self.state.arm_position_command)
# # print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True))
# # print("delta_pose:",delta_pose)
# # print("delta_ori_mat",delta_ori_mat)
# # self.laset_print_time = time.time()

1432
logs/MassageRobot_nova5_test.log
View File

File diff suppressed because it is too large Load Diff