优化STEP算法，降低CPU使用率
This commit is contained in:
Ziwei.He
parent
3b37aedae2
commit
e1583b1756
Binary file not shown.
@ -40,73 +40,84 @@ class AdmittanceController(BaseController):
|
||||
self.laset_print_time = 0
|
||||
|
||||
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:
|
||||
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:
|
||||
# print("pose_error:",self.state.pose_error[:3])
|
||||
# 计算误差 位置直接作差，姿态误差以旋转向量表示
|
||||
#rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
|
||||
rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
|
||||
# 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)
|
||||
self.state.pose_error[3:] = -rot_err_rotvex
|
||||
|
||||
#wrench_err = self.state.external_wrench_base - self.state.desired_wrench
|
||||
# 缓存常用计算
|
||||
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
|
||||
# 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 = 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)
|
||||
KD_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
|
||||
KD_pose = self.K @ self.state.pose_error
|
||||
self.state.arm_desired_acc = self.M_inv @ (wrench_err - KD_vel - KD_pose)
|
||||
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.state.arm_desired_twist += self.state.arm_desired_acc * dt
|
||||
self.clip_command(self.state.arm_desired_twist, "vel")
|
||||
delta_pose = self.state.arm_desired_twist * dt
|
||||
|
||||
# delta_pose[:3] = self.pos_scale_factor * delta_pose[:3]
|
||||
# delta_pose[3:] = self.rot_scale_factor * delta_pose[3:]
|
||||
# if time.time() - self.laset_print_time > 5:
|
||||
# print("delta_pose:",delta_pose)
|
||||
|
||||
delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
|
||||
|
||||
# if time.time() - self.laset_print_time > 5:
|
||||
# print("tf_delta_pose:",delta_pose)
|
||||
# 计算位姿变化
|
||||
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")
|
||||
|
||||
# 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()
|
||||
# 更新四元数
|
||||
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 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()
|
||||
|
||||
|
||||
# # 计算误差 位置直接作差，姿态误差以旋转向量表示
|
||||
# temp_pose_error = self.state.arm_position - self.state.desired_position
|
||||
# if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
||||
# 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
|
||||
# rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
|
||||
# rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
|
||||
# self.state.pose_error[3:] = -rot_err_rotvex
|
||||
# wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
|
||||
# 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")
|
||||
# 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()
|
||||
|
||||
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user