stash

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"

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)
     

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):
     #     # 计算误差 位置直接作差，姿态误差以旋转向量表示

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()
+    # # dobot.start_drag()
+    # dobot.disableRobot()

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