From 961a86d42305f101b7605943049ce9f24ef67d60 Mon Sep 17 00:00:00 2001 From: LiangShiyun <15723182159@163.com> Date: Fri, 30 May 2025 22:12:09 +0800 Subject: [PATCH] stash --- Massage/MassageControl/MassageRobot_nova5.py | 46 ++++++- .../algorithms/admittance_controller.py | 120 +++++++++++++----- .../MassageControl/hardware/dobot_nova5.py | 70 ++++++++-- 3 files changed, 187 insertions(+), 49 deletions(-) diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 24e0b38..6deced9 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -247,7 +247,9 @@ class MassageRobot: return x_update, P_update def init_hardwares(self,ready_pose): self.ready_pose = np.array(ready_pose) - self.switch_payload(self.current_head) + self.arm_state.desired_position = self.ready_pose[:3] + euler_angles = self.ready_pose[3:] + self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat() print(self.arm.get_arm_position()) time.sleep(0.5) @@ -551,12 +553,15 @@ class MassageRobot: if sleep_duration > 0: time.sleep(sleep_duration) # print(f"real sleep:{time.time()-b2}") - + + # print("self111:",self.arm.feedbackData.robotMode) self.arm.dashboard.socket_dobot.sendall(tcp_command) + # print("self111222:",self.arm.feedbackData.robotMode) if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState: self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}") self.arm.dashboard.Stop() self.arm.dashboard.EmergencyStop(mode=1) + self.stop() return -1 if self.arm.feedbackData.EnableState and self.arm.feedbackData.robotMode == 10: if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停 @@ -591,11 +596,12 @@ class MassageRobot: self.arm_state.arm_orientation = quat_rot for i in range(20): self.controller_manager.step(self.control_rate.to_sec()) - self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}") - self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}") + # self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}") + # self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}") position, quat_rot = self.arm.get_arm_position() - self.logger.log_blue(f"position current:{position}") - self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}") + # self.logger.log_blue(f"position current:{position}") + # self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}") + print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode) self.command_rate.precise_sleep() position ,quat_rot = self.arm.get_arm_position() @@ -1613,6 +1619,26 @@ class MassageRobot: 'wrench': wrench_traj } return traj + + # 工具函数 + ############################################################################################################ + def convert_to_7d(self,matrix): + matrix = np.array(matrix) + positions = matrix[:, :3] + rpy_angles = matrix[:, 3:] + # 将RPY角度转换为四元数 + quaternions = R.from_euler('xyz', rpy_angles).as_quat() + # 将位置和四元数组合成一个新的矩阵 + result_matrix = np.hstack([positions, quaternions]) + # 加入当前位置 + current_quat = self.arm_state.arm_orientation + current_position = self.arm_state.arm_position + if quaternions[0].dot(current_quat) < 0: + current_quat = -current_quat + current_pose = np.hstack([current_position,current_quat]) + result_matrix = np.vstack([current_pose,result_matrix]) + return result_matrix + @@ -1645,7 +1671,7 @@ if __name__ == "__main__": # robot.dt = ts[1] - ts[0] - ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] + ready_pose = [0.1543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180] robot.current_head = 'finger_head' robot.force_sensor.disable_active_transmission() @@ -1660,7 +1686,13 @@ if __name__ == "__main__": robot.init_hardwares(ready_pose) time.sleep(3) + robot.switch_payload("none") robot.controller_manager.switch_controller('admittance') + position,quat_rot = robot.arm.get_arm_position() + euler_rot = R.from_quat(quat_rot).as_euler('xyz') + target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]] + robot.move_to_point(pose=target_point,t=60) + # robot.controller_manager.switch_controller('position') diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py index 05952f7..f6a218c 100644 --- a/Massage/MassageControl/algorithms/admittance_controller.py +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -40,42 +40,100 @@ class AdmittanceController(BaseController): 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 + # D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist) + # K_pose = self.K @ self.state.pose_error + # self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose) + # 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(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 - # 缓存常用计算 - 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) - # 期望加速度 + + 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 wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench - D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist) - K_pose = self.K @ self.state.pose_error - self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose) - 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) - # 更新位置 + if time.time() - self.laset_print_time > 5: + 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) + # 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") + 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] = 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) + self.clip_command(delta_pose,"pose") + + # testlsy + delta_ori_mat = R.from_rotvec(delta_pose[3:]).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() + + # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:] + # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat() + + # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + + + # if time.time() - self.laset_print_time > 1: + # print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position) def step_traj(self,dt): # 方向统一 diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index b5b72dc..4a2b88e 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -150,6 +150,10 @@ class dobot_nova5: atexit.register(self.exit_task) self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180] self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180] + self.is_exit = False + self.is_standby = False + self.filter_matirx = np.zeros((1,7)) # 位姿伺服用 + self.filter_matrix = np.zeros((1,6)) # 角度伺服用 # 为状态管理而封装的初始化函数 def init(self): @@ -164,6 +168,8 @@ class dobot_nova5: self.filter_matrix = np.zeros((1,6)) # 角度伺服用 sleep(1) self.is_standby = False + self.move_joint_jog(q=self.init_pos) + time.sleep(0.5) code = self.move_joint(self.init_pos) if code == 0: @@ -275,14 +281,16 @@ class dobot_nova5: def waitArrival(self, command): sendCommandID = self.parseResultId(command)[1] + # print("self.parseResultId(command)",self.parseResultId(command)) while True: - if self.feedbackData.robotMode in [3, 4, 6, 8, 9, 10, 11]: + if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]: self.logger.log_error(f"机械臂异常:{self.feedbackData.robotMode}") print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式")) self.dashboard.Stop() self.dashboard.EmergencyStop(mode=1) return -1 if self.feedbackData.EnableState: + print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID) if self.feedbackData.robotCurrentCommandID > sendCommandID: break else: @@ -314,7 +322,32 @@ class dobot_nova5: return -1 print(f'After {cnt} retries, still failed to arrive at the joint position: {q}') return -1 - + + def move_joint_jog(self,q,**params): + ''' + move_joint_jog 以JOINT jOG运动方式运动到目标点 + MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6] unit: rad + ''' + threshold = params.get('threshold', 10) + currentAngle = self.getAngel() + targetAngle = self.__transform_rad_2_deg(q) + currentAngle = np.array(currentAngle) + targetAngle = np.array(targetAngle) + self.dashboard.SpeedFactor(70) + + for i in range(len(targetAngle)): + while np.abs(currentAngle[i] - targetAngle[i]) >threshold: + if currentAngle[i] < targetAngle[i]: + self.dashboard.MoveJog(f"J{i+1}+") + else: + self.dashboard.MoveJog(f"J{i+1}-") + time.sleep(0.5) + + currentAngle = self.getAngel() + currentAngle = np.array(currentAngle) + self.dashboard.MoveJog("") + # self.dashboard.SpeedFactor(95) + def RunPoint_P_inPose_M_RAD(self,pose,**params): ''' RUNPOINT_P 以关节运动方式运动到目标点 @@ -810,16 +843,31 @@ class dobot_nova5: if __name__ == "__main__": # sleep(5) + dobot = dobot_nova5("192.168.5.1") - dobot.start() - posJ = [0,30,-120,0,90,0] - dobot.RunPoint_P_inJoint(posJ) - sleep(1) - dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") - dobot.chooseEndEffector(i=9) - print("Arm pose:",dobot.getPose()) + dobot.init() - # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) + # dobot.move_joint_jog(q=dobot.init_pos) + # dobot.dashboard.MoveJog("J1+") + # time.sleep(1) + # dobot.dashboard.MoveJog("") + # print(dobot.getAngel()) + # # while True: + # # time.sleep(1) + + # # dobot.start() + # posJ = [0,30,-120,0,90,0] # dobot.start_drag() - dobot.disableRobot() \ No newline at end of file + # time.sleep(60) + # dobot.stop_drag() + # # dobot.RunPoint_P_inJoint(posJ) + # # sleep(1) + # # dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") + # # dobot.chooseEndEffector(i=9) + # # print("Arm pose:",dobot.getPose()) + + # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) + + # # dobot.start_drag() + # dobot.disableRobot() \ No newline at end of file