diff --git a/Massage/Massage.py b/Massage/Massage.py index f0fc734..587b138 100755 --- a/Massage/Massage.py +++ b/Massage/Massage.py @@ -1109,7 +1109,7 @@ class SharedResources: return "emergency" time.sleep(1) self.cam_position,self.cam_quaternion = self.robot.arm.get_end_position() - self.logger.log_info(f"获取最新的位置:{self.cam_position}和姿态:{self.cam_quaternion}") + self.logger.log_info(f"获取cam的位置:{self.cam_position}和姿态:{self.cam_quaternion}") if self.stop_event.is_set(): self.logger.log_info(f"检测到停止事件") return "stop" diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 4249e44..73bf87e 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -209,7 +209,7 @@ class MassageRobot: self.tool_position = None self.mass_center_position = None self.s_tf_matrix_t = None - arm_orientation_set0 = np.array([-180,0,-90]) + arm_orientation_set0 = np.array([-180,-30,0]) self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix() @@ -406,6 +406,7 @@ class MassageRobot: if name in self.playload_dict: self.stop() self.current_head = name + self.logger.log_blue(f"Into switch_payload,change to{name}") compensation_config = self.playload_dict[self.current_head] @@ -416,16 +417,23 @@ class MassageRobot: self.tool_position = np.array(compensation_config['tcp_offset']) self.mass_center_position = np.array(compensation_config['mass_center_position']) self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat()) - - tcp_offset = self.playload_dict[name]["tcp_offset"] - tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}" - print("tcp_offset_str",tcp_offset_str) - self.tool_num = 9 - self.arm.setEndEffector(i=self.tool_num,tool_i=tcp_offset_str) - self.arm.chooseEndEffector(i=self.tool_num) - print(self.arm.get_arm_position()) - self.logger.log_info(f"切换到{name}按摩头") + self.logger.log_blue(f"current position and orientation:{self.arm_state.arm_position},{self.arm_state.arm_orientation}") + self.logger.log_yellow("---------------------change----------------------") + self.arm.change_end_effector(self.playload_dict[name]) + self.logger.log_info(f"切换到{name}按摩头") + R_matrix = R.from_euler('xyz',self.ready_pose[3:] ,degrees=False).as_matrix() + ready_position = self.ready_pose[:3] + R_matrix @ self.playload_dict[self.current_head]['tcp_offset'][:3] + ready_orientation = R_matrix @ R.from_euler('xyz',self.playload_dict[self.current_head]['tcp_offset'][3:],degrees=False).as_matrix() + ready_orientation_euler = R.from_matrix(ready_orientation).as_euler('xyz',degrees=False) + print("ready_pose",self.ready_pose) + print("desired_position", ready_position) + print("desired_orientation",ready_orientation_euler) + # TODO + self.arm_state.desired_position = ready_position + self.arm_state.desired_orientation = R.from_euler('xyz',ready_orientation_euler,degrees=False).as_quat() + self.controller_manager.switch_controller('position') + # ####################### 位姿伺服 ########################## @@ -534,6 +542,7 @@ class MassageRobot: process_start_time = time.time() control_cycle = self.control_rate.to_sec() self.controller_manager.step(control_cycle) + command_pose = np.concatenate([ self.arm_state.arm_position_command * 1000, # 转毫米 self.arm_state.arm_orientation_command @@ -543,6 +552,15 @@ class MassageRobot: command_pose[:3], command_pose[3:] ) + + temp_pose = copy.deepcopy(pose_processed) + + # print("self.arm.tcp_euler_inv:",self.arm.tcp_euler_inv,temp_pose) + + target_matrix = R.from_euler('xyz', temp_pose[3:]*np.pi/180, degrees=False).as_matrix() @ self.arm.tcp_euler_inv + pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])*1000 + pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = True) + # print(f"pose_processed:{pose_processed}") tcp_command = ( @@ -606,7 +624,7 @@ class MassageRobot: 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)}") - print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode) + # print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode) self.command_rate.precise_sleep() position ,quat_rot = self.arm.get_arm_position() @@ -1707,26 +1725,36 @@ if __name__ == "__main__": # target_point = [0.3543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180] # robot.move_to_point(pose=target_point,t=60) - robot.arm.move_joint(q = robot.arm.off_pos) - robot.arm.move_joint(q = robot.arm.init_pos) + # robot.arm.move_joint(q = robot.arm.off_pos) + # robot.arm.move_joint(q = robot.arm.init_pos) pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180] - robot.set_position(pose = pose1) - robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0]) + # robot.set_position(pose = pose1) + print("arm_position1:",robot.arm.get_arm_position()) + print("end_position1:",robot.arm.get_end_position()) + print("joint1:",robot.arm.getAngel()) + robot.arm.move_joint(q=robot.arm.init_pos) + # robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0]) + print("arm_position2:",robot.arm.get_arm_position()) + print("end_position2:",robot.arm.get_end_position()) + print("joint2:",robot.arm.getAngel()) + # robot.controller_manager.switch_controller('position') atexit.register(robot.force_sensor.disconnect) - robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0]) + # robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0]) try: - robot_thread = threading.Thread(target=robot.start) - robot_thread.start() + # robot_thread = threading.Thread(target=robot.start) + # robot_thread.start() + while True: + time.sleep(1) except KeyboardInterrupt: - robot_thread.join() + # robot_thread.join() print("用户中断操作。") except Exception as e: - robot_thread.join() + # robot_thread.join() print("Exception occurred at line:", e.__traceback__.tb_lineno) print("发生异常：", e) diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py index 86cc6b8..4363219 100644 --- a/Massage/MassageControl/algorithms/admittance_controller.py +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -81,8 +81,8 @@ class AdmittanceController(BaseController): # 更新位置 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_desired_acc) + # if time.time() - self.laset_print_time > 1: + # print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_desired_acc) # def step(self,dt): # # 计算误差 位置直接作差，姿态误差以旋转向量表示 diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index 2508297..2ef4cf1 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -52,6 +52,8 @@ import atexit class dobot_nova5: def __init__(self,arm_ip = '192.168.5.1',dashboardPort = 29999,feedFourPort = 30004): self.ip = arm_ip ## 机械臂IP地址 + self.tcp_offset = [0, 0, 0, 0, 0, 0] # 工具坐标系偏移 + self.tcp_euler_inv = np.linalg.inv(R.from_euler('xyz', self.tcp_offset[3:], degrees=False).as_matrix()) self.dashboardPort = dashboardPort ## 一发一收-设置-控制及运动端口号 self.feedFourPort = feedFourPort ## 第四实时反馈(8ms)反馈数据端口号 self.dashboard = None ## 初始化控制及运动端口实例 @@ -160,6 +162,7 @@ class dobot_nova5: self.filter_matirx = np.zeros((1,7)) # 位姿伺服用 self.filter_matrix = np.zeros((1,6)) # 角度伺服用 + # 为状态管理而封装的初始化函数 def init(self): self.is_exit = False @@ -208,6 +211,11 @@ class dobot_nova5: self.last_print_time=0 self.last_reocrd_time =0 + def change_end_effector(self, end_effector_config): + # 设置tcp偏移 + self.tcp_offset = end_effector_config["tcp_offset"] + self.tcp_euler_inv = np.linalg.inv(R.from_euler('xyz', self.tcp_offset[3:], degrees=False).as_matrix()) + def exit_task(self): self.logger.log_yellow("退出任务") # self.disable() @@ -220,6 +228,8 @@ class dobot_nova5: ) time.sleep(1) self.move_joint(self.off_pos) + self.stop_motion() + self.disableRobot() self.stop_feedback.set() self.feedback_thread.join() self.dashboard.Disconnect() @@ -286,7 +296,7 @@ class dobot_nova5: def waitArrival(self, command): sendCommandID = self.parseResultId(command)[1] - print("self.feedbackData.robotMode6:",self.feedbackData.robotMode) + # print("self.feedbackData.robotMode6:",self.feedbackData.robotMode) # print("self.parseResultId(command)",self.parseResultId(command)) while True: if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]: @@ -579,7 +589,7 @@ class dobot_nova5: start = res.find("{")+1 end = res.find("}") angle_str = res[start:end] - print("res:",res) + # print("res:",res) angle = [float(x.strip()) for x in angle_str.split(",")] return angle @@ -751,14 +761,15 @@ class dobot_nova5: quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat() return pos,quat_rot - def get_end_position(self,tool=1): + def get_end_position(self,tool=0): ''' 工具默认为1 输出pos,quat pos: [x,y,z] in m quat: [qx,qy,qz,qw] 四元数 ''' - pose = self.getPose(user=1, tool=tool) + # pose = self.getPose(user=1, tool=tool) + pose = self.Get_feedInfo_pose() x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree x /= 1000 y /= 1000 @@ -776,15 +787,31 @@ class dobot_nova5: pos: [x,y,z] in m quat: [qx,qy,qz,qw] 四元数 ''' + tool = 0 pose = self.Get_feedInfo_pose() + # pose = self.getPose(user=1, tool=tool) # print("pose",pose) x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree x /= 1000 y /= 1000 z /= 1000 pos = np.array([x,y,z]) - quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat() - return pos,quat_rot + ori = np.array([rx, ry, rz])*np.pi/180 + + try: + if self.tcp_offset == None: + self.tcp_offset = [0, 0, 0, 0, 0, 0] # 工具坐标系偏移 + # 添加tcp偏移 + target_pos = pos + R.from_euler("xyz",ori,degrees=False).as_matrix() @ np.array(self.tcp_offset[:3]) + target_matrix = R.from_euler('xyz', ori, degrees=False).as_matrix() @ R.from_euler('xyz', self.tcp_offset[3:], degrees=False).as_matrix() + target_euler = R.from_matrix(target_matrix).as_euler('xyz', degrees=False) + + tool_position = target_pos + tool_quat_rot = R.from_euler('xyz', target_euler, degrees=False).as_quat() + return tool_position, tool_quat_rot + except Exception as e: + self.logger.log_error(f"计算工具位置时发生错误：{str(e)}") + return None, None def __circular_mean(self,angles): # 将角度转换为复数单位圆上的点，再计算均值 @@ -860,32 +887,32 @@ if __name__ == "__main__": dobot = dobot_nova5("192.168.5.1") dobot.init() - # dobot.move_joint_jog(q=dobot.init_pos) + # # dobot.move_joint_jog(q=dobot.init_pos) - # dobot.dashboard.MoveJog("J1+") + # # dobot.dashboard.MoveJog("J1+") + # # time.sleep(1) + # # dobot.dashboard.MoveJog("") + # print("dobot.getangle::",dobot.getAngel()) + # # # while True: + # # # time.sleep(1) + + # # # dobot.start() + # # posJ = [0,30,-120,0,90,0] + # # time.sleep(1) + # joint = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180] + # # dobot.move_joint(q = joint) # time.sleep(1) - # dobot.dashboard.MoveJog("") - print("dobot.getangle::",dobot.getAngel()) - # # while True: - # # time.sleep(1) - # # dobot.start() - # posJ = [0,30,-120,0,90,0] - # time.sleep(1) - joint = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180] - # dobot.move_joint(q = joint) - time.sleep(1) + # joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180] + # try: + # dobot.start_drag() + # # dobot.move_joint(q = joint) + # # dobot.move_joint(q = joint1) - joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180] + # # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180] + + # # dobot.RunPoint_P_inPose_M_RAD(pose=pose1) try: - dobot.start_drag() - # dobot.move_joint(q = joint) - # dobot.move_joint(q = joint1) - - # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180] - - # dobot.RunPoint_P_inPose_M_RAD(pose=pose1) - while True: time.sleep(1) except KeyboardInterrupt: @@ -895,13 +922,13 @@ if __name__ == "__main__": # robot_thread.join() print("Exception occurred at line:", e.__traceback__.tb_lineno) print("发生异常：", e) - # 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_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.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) - # dobot.start_drag() - dobot.disableRobot() \ No newline at end of file + # # dobot.start_drag() + # dobot.disableRobot() \ No newline at end of file diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_0.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_0.jpg index c4ec39e..dade6fe 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_0.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_0.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_1.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_1.jpg index c4ec39e..dade6fe 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_1.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_1.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_10.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_10.jpg index 1942503..a2d29c4 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_10.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_10.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_11.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_11.jpg index 18c4324..c36de05 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_11.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_11.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_12.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_12.jpg index dc6ad16..636700c 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_12.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_12.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_13.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_13.jpg index 39af8a4..e9301a1 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_13.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_13.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_14.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_14.jpg index caa5a8d..e4cf807 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_14.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_14.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_15.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_15.jpg index 24a18bd..2dd5143 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_15.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_15.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_16.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_16.jpg index 43ffb77..1638c00 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_16.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_16.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_17.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_17.jpg index 485f6cf..dade6fe 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_17.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_17.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_18.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_18.jpg index 84c931b..2997560 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_18.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_18.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_19.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_19.jpg index 9b74938..80e9edc 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_19.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_19.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_2.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_2.jpg index 7cd9f3b..2997560 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_2.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_2.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_20.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_20.jpg index 7e0de44..df65a3e 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_20.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_20.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_21.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_21.jpg index 664e3bd..9783ac5 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_21.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_21.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_22.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_22.jpg index 1942503..484c867 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_22.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_22.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_23.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_23.jpg index 18c4324..b01040a 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_23.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_23.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_24.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_24.jpg index dc6ad16..ae087c4 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_24.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_24.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_25.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_25.jpg index 39af8a4..c24b0db 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_25.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_25.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_26.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_26.jpg index caa5a8d..d2a1373 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_26.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_26.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_27.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_27.jpg index 24a18bd..2feb60a 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_27.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_27.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_28.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_28.jpg index 43ffb77..9eb18c8 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_28.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_28.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_29.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_29.jpg index 485f6cf..a02f8c4 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_29.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_29.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_3.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_3.jpg index 80e9edc..9072d55 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_3.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_3.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_30.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_30.jpg index 84c931b..3830cfe 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_30.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_30.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_31.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_31.jpg index 9b74938..793241e 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_31.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_31.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_4.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_4.jpg index df65a3e..721f702 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_4.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_4.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_5.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_5.jpg index 9783ac5..ccf29c9 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_5.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_5.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_6.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_6.jpg index 61cff9e..8227f5f 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_6.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_6.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_7.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_7.jpg index bd3d2cd..52e25f8 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_7.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_7.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_8.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_8.jpg index 7e0de44..77db08b 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_8.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_8.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_9.jpg b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_9.jpg index 664e3bd..b09aab0 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_9.jpg and b/Massage/aucpuncture2point/configs/using_img/Pixel_Visualization_9.jpg differ diff --git a/Massage/aucpuncture2point/configs/using_img/back_img/back_acupoints.png b/Massage/aucpuncture2point/configs/using_img/back_img/back_acupoints.png index b4f0a9d..15fbea4 100644 Binary files a/Massage/aucpuncture2point/configs/using_img/back_img/back_acupoints.png and b/Massage/aucpuncture2point/configs/using_img/back_img/back_acupoints.png differ diff --git a/Massage/aucpuncture2point/configs/using_img/color.png b/Massage/aucpuncture2point/configs/using_img/color.png index 44c5072..190772d 100755 Binary files a/Massage/aucpuncture2point/configs/using_img/color.png and b/Massage/aucpuncture2point/configs/using_img/color.png differ diff --git a/Massage/aucpuncture2point/configs/using_img/depth.png b/Massage/aucpuncture2point/configs/using_img/depth.png index b75a134..0c4d4ae 100755 Binary files a/Massage/aucpuncture2point/configs/using_img/depth.png and b/Massage/aucpuncture2point/configs/using_img/depth.png differ diff --git a/Massage/aucpuncture2point/scripts/coordinate_transform.py b/Massage/aucpuncture2point/scripts/coordinate_transform.py index cd664a4..22e9896 100644 --- a/Massage/aucpuncture2point/scripts/coordinate_transform.py +++ b/Massage/aucpuncture2point/scripts/coordinate_transform.py @@ -345,6 +345,7 @@ class CoordinateTransformer: camera_coordinate = np.array(camera_coordinate) # 将相机坐标系转换到机械臂末端坐标系 arm_coordinate = [0, 0, 0] + camera_matrix = copy.deepcopy(self.camera_matrix) camera_trans = copy.deepcopy(self.camera_trans) # if self.camera_matrix is not None and self.camera_trans is not None: @@ -356,6 +357,7 @@ class CoordinateTransformer: arm_coordinate = camera_matrix @ (camera_coordinate/1000) + camera_trans # 将机械臂末端坐标系转为机械臂基座坐标系 targetPosition = R.from_quat(self.quaternion).as_matrix() @ arm_coordinate + self.position + print("camera_coordinate:",camera_coordinate,camera_matrix,camera_trans,arm_coordinate,targetPosition) return targetPosition.tolist() def calculate_arm_normal(self, data): @@ -367,6 +369,8 @@ class CoordinateTransformer: # 机械臂末端坐标系到基座坐标系 transformed_normal = R.from_quat(self.quaternion).as_matrix() @ arm_normal + + print("transformed_normal:",arm_normal,transformed_normal) return { 'label': data['label'], @@ -377,6 +381,7 @@ class CoordinateTransformer: """计算目标欧拉角""" converted_points = [] + for point_data in points: point = np.array(point_data['normals_position'], dtype=np.float64).reshape(1, -1) temp_point = np.zeros((1, 6), dtype=np.float64) @@ -384,9 +389,11 @@ class CoordinateTransformer: # 复制位置坐标 temp_point[:, :3] = point[:, :3] - - temp_point[0][3] = math.atan2(point[0][3], point[0][5]) - temp_point[0][4] = -math.asin(point[0][4]) + if point[0][5]<= 0: + temp_point[0][3] = -math.atan2(point[0][4],-np.sqrt(1-point[0][4]*point[0][4])) + else: + temp_point[0][3] = -math.atan2(point[0][4],np.sqrt(1-point[0][4]*point[0][4])) + temp_point[0][4] = -math.atan2(-point[0][5], point[0][3]) temp_point[0][5] = 0 # 确保角度在[-pi, pi]范围内 @@ -397,6 +404,8 @@ class CoordinateTransformer: 'label': point_data['label'], 'position': temp_point[0].tolist() }) + + print("target euler_point:",points,converted_points) return converted_points diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg index c4ec39e..dade6fe 100644 Binary files a/UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg and b/UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg index c4ec39e..dade6fe 100644 Binary files a/UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg and b/UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_10.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_10.jpg new file mode 100644 index 0000000..a2d29c4 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_10.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_11.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_11.jpg new file mode 100644 index 0000000..c36de05 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_11.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_12.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_12.jpg new file mode 100644 index 0000000..636700c Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_12.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_13.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_13.jpg new file mode 100644 index 0000000..e9301a1 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_13.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_14.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_14.jpg new file mode 100644 index 0000000..e4cf807 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_14.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_15.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_15.jpg new file mode 100644 index 0000000..2dd5143 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_15.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_16.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_16.jpg new file mode 100644 index 0000000..1638c00 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_16.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_17.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_17.jpg new file mode 100644 index 0000000..dade6fe Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_17.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_18.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_18.jpg new file mode 100644 index 0000000..7cd9f3b Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_18.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_19.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_19.jpg new file mode 100644 index 0000000..80e9edc Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_19.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg index 7cd9f3b..2997560 100644 Binary files a/UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg and b/UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_20.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_20.jpg new file mode 100644 index 0000000..df65a3e Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_20.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_21.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_21.jpg new file mode 100644 index 0000000..9783ac5 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_21.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_22.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_22.jpg new file mode 100644 index 0000000..484c867 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_22.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_23.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_23.jpg new file mode 100644 index 0000000..b01040a Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_23.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_24.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_24.jpg new file mode 100644 index 0000000..ae087c4 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_24.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_25.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_25.jpg new file mode 100644 index 0000000..c24b0db Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_25.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_26.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_26.jpg new file mode 100644 index 0000000..d2a1373 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_26.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_27.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_27.jpg new file mode 100644 index 0000000..2feb60a Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_27.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_28.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_28.jpg new file mode 100644 index 0000000..9eb18c8 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_28.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_29.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_29.jpg new file mode 100644 index 0000000..a02f8c4 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_29.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg index 80e9edc..9072d55 100644 Binary files a/UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg and b/UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_30.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_30.jpg new file mode 100644 index 0000000..3830cfe Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_30.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg index df65a3e..721f702 100644 Binary files a/UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg and b/UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_5.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_5.jpg new file mode 100644 index 0000000..ccf29c9 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_5.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_6.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_6.jpg new file mode 100644 index 0000000..8227f5f Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_6.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_7.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_7.jpg new file mode 100644 index 0000000..52e25f8 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_7.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_8.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_8.jpg new file mode 100644 index 0000000..77db08b Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_8.jpg differ diff --git a/UI_next/static/images/tmp_images/Pixel_Visualization_9.jpg b/UI_next/static/images/tmp_images/Pixel_Visualization_9.jpg new file mode 100644 index 0000000..b09aab0 Binary files /dev/null and b/UI_next/static/images/tmp_images/Pixel_Visualization_9.jpg differ diff --git a/UI_next/static/images/tmp_images/back_acupoints.png b/UI_next/static/images/tmp_images/back_acupoints.png index b4f0a9d..15fbea4 100644 Binary files a/UI_next/static/images/tmp_images/back_acupoints.png and b/UI_next/static/images/tmp_images/back_acupoints.png differ diff --git a/UI_next/static/images/tmp_images/color.png b/UI_next/static/images/tmp_images/color.png index 44c5072..190772d 100755 Binary files a/UI_next/static/images/tmp_images/color.png and b/UI_next/static/images/tmp_images/color.png differ