tried to match the meshes

workspaces/COLCON_WS/src/ser_test/ser_test/test.py

@@ -11,35 +11,71 @@ class TransformMeshNode(Node):
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
         self.timer = self.create_timer(1.0, self.timer_callback)
-        self.mesh_shoulder_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl')
+        self.mesh_shoulder_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/shoulder.stl')
         self.mesh_base_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/base.stl')
         self.mesh_forearm_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/forearm.stl')
-        self.mesh_upperarm_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/upperarm.stl')
+        self.mesh_upperarm_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/upperarm.stl')
         self.mesh_wrist1_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist1.stl')
         self.mesh_wrist2_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist2.stl')
         self.mesh_wrist3_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist3.stl')
         self.source_frame = 'vl53l7cx_link'
     def timer_callback(self):
-        axis_length = 0.1
+        axis_length = 0.2
         axes = {
-            'shoulder': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'forearm': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'base': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'upperarm': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'wrist1': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'wrist2': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
-            'wrist3': trimesh.creation.axis(origin_size=0.01, axis_length=axis_length),
+            'shoulder': trimesh.creation.axis(origin_size=0.03, axis_length=axis_length),
+            'forearm': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
+            'base': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
+            'upperarm': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
+            'wrist1': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
+            'wrist2': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
+            'wrist3': trimesh.creation.axis(origin_size=0.02, axis_length=axis_length),
         }
 
         try:
+
+            rotation_matrix_180_1 = trimesh.transformations.rotation_matrix(np.pi, [0, 0, 1])
+            rotation_matrix_180_y = trimesh.transformations.rotation_matrix(np.pi, [0, 0.5, 0])
+            #self.mesh_base_link.apply_transform(rotation_matrix_180_1)
+            #self.mesh_base_link.apply_transform(rotation_matrix_180_y)
+            #axes['base'].apply_transform(rotation_matrix_180_y)
+
+            rotation_matrix_90_x = trimesh.transformations.rotation_matrix(-np.pi, [0, 0, 0.5])
+
+            rotation_matrix_90_z = trimesh.transformations.rotation_matrix(np.pi / 2, [0, 0, 0.5])
+            #self.mesh_upperarm_link.apply_transform(rotation_matrix_90_z)
+            #axes['upperarm'].apply_transform(rotation_matrix_90_z)
+
+            rotation_matrix_90_y = trimesh.transformations.rotation_matrix(np.pi / 2, [0.5, 0, 0])
+            #self.mesh_upperarm_link.apply_transform(rotation_matrix_90_y)
+            #axes['upperarm'].apply_transform(rotation_matrix_90_y)
+
+            #self.mesh_upperarm_link.apply_transform(rotation_matrix_90_x)
+            #axes['upperarm'].apply_transform(rotation_matrix_90_x)
+
+            #self.mesh_upperarm_link.apply_translation([0.1762, 0, 0])
+            #axes['upperarm'].apply_translation([0.1762, 0, 0])
+
+
+
             now = rclpy.time.Time()
-            transform_shoulder = self.tf_buffer.lookup_transform('shoulder_link', self.source_frame, now)
-            transform_forearm = self.tf_buffer.lookup_transform('forearm_link', self.source_frame, now)
-            transform_base = self.tf_buffer.lookup_transform('base_link', self.source_frame, now)
-            transform_upperarm = self.tf_buffer.lookup_transform('upper_arm_link', self.source_frame, now)
-            transform_wrist1 = self.tf_buffer.lookup_transform('wrist_1_link', self.source_frame, now)
-            transform_wrist2 = self.tf_buffer.lookup_transform('wrist_2_link', self.source_frame, now)
-            transform_wrist3 = self.tf_buffer.lookup_transform('wrist_3_link', self.source_frame, now)
+            transform_base = self.tf_buffer.lookup_transform('base_link', 'world', now)
+            transform_shoulder = self.tf_buffer.lookup_transform('base_link', 'shoulder_link', now)
+            #transform_shoulder = self.swap_x_and_z(transform_shoulder)
+
+            transform_upperarm = self.tf_buffer.lookup_transform('base_link', 'upper_arm_link', now)
+            #transform_upperarm = self.swap_x_and_z(transform_upperarm)    
+            #transform_upperarm = self.swap_y_and_z_rotation(transform_upperarm)
+            #transform_upperarm = self.swap_x_and_y_rotation(transform_upperarm)
+               
+            #transform_upperarm_rotation = self.invert_y_rotation(transform_upperarm_rotation)
+            #transform_upperarm = self.invert_z_translation(transform_upperarm)
+            self.get_euler_and_translation(transform_shoulder)
+            self.get_euler_and_translation(transform_upperarm)
+
+            transform_forearm = self.tf_buffer.lookup_transform('forearm_link', 'base_link', now)
+            transform_wrist1 = self.tf_buffer.lookup_transform('wrist_1_link', 'base_link', now)
+            transform_wrist2 = self.tf_buffer.lookup_transform('wrist_2_link', 'base_link', now)
+            transform_wrist3 = self.tf_buffer.lookup_transform('wrist_3_link', 'base_link', now)
 
             self.transform_mesh(transform_shoulder, self.mesh_shoulder_link)
             self.transform_mesh(transform_forearm, self.mesh_forearm_link)
@@ -57,32 +93,110 @@ class TransformMeshNode(Node):
             self.transform_mesh(transform_wrist2, axes['wrist2'])
             self.transform_mesh(transform_wrist3, axes['wrist3'])
 
-            rotation_matrix_180_x = trimesh.transformations.rotation_matrix(np.pi, [0, 0, 1])
-            self.mesh_shoulder_link.apply_transform(rotation_matrix_180_x)
-            axes['shoulder'].apply_transform(rotation_matrix_180_x)
-
             scene = trimesh.Scene()
-            scene.add_geometry(self.mesh_shoulder_link)
-            scene.add_geometry(self.mesh_forearm_link)
+            world_axis = trimesh.creation.axis(origin_size=0.02, axis_length=0.4)
+            #scene.add_geometry(world_axis)
             scene.add_geometry(self.mesh_base_link)
-            scene.add_geometry(self.mesh_upperarm_link)
-            scene.add_geometry(self.mesh_wrist1_link)
-            scene.add_geometry(self.mesh_wrist2_link)
-            scene.add_geometry(self.mesh_wrist3_link)
+            scene.add_geometry(self.mesh_shoulder_link)
+            #scene.add_geometry(self.mesh_upperarm_link)
+            #scene.add_geometry(self.mesh_forearm_link)
+            #scene.add_geometry(self.mesh_wrist1_link)
+            #scene.add_geometry(self.mesh_wrist2_link)
+            #scene.add_geometry(self.mesh_wrist3_link)
 
             scene.add_geometry(axes['shoulder'])
-            scene.add_geometry(axes['forearm'])
+            #scene.add_geometry(axes['forearm'])
             scene.add_geometry(axes['base'])
-            scene.add_geometry(axes['upperarm'])
-            scene.add_geometry(axes['wrist1'])
-            scene.add_geometry(axes['wrist2'])
-            scene.add_geometry(axes['wrist3'])
+            #scene.add_geometry(axes['upperarm'])
+            #scene.add_geometry(axes['wrist1'])
+            #scene.add_geometry(axes['wrist2'])
+            #scene.add_geometry(axes['wrist3'])
 
             scene.show()
 
         except Exception as e:
             self.get_logger().warn(f'Could not transform: {e}')
+
+
     def transform_mesh(self, transform, mesh):
+        # Extract the translation and rotation from the transform
+        translation = transform.transform.translation
+        rotation = transform.transform.rotation
+
+        # Create a translation matrix
+        translation_matrix = trimesh.transformations.translation_matrix([translation.x, translation.y, translation.z])
+
+        # Create a rotation matrix from the quaternion
+        rotation_matrix = trimesh.transformations.quaternion_matrix([rotation.w, rotation.x, rotation.y, rotation.z])
+
+        # Combine the translation and rotation matrices into a single transformation matrix
+        transformation_matrix = np.dot(translation_matrix, rotation_matrix)
+        # Apply the transformation matrix to the mesh
+        mesh.apply_transform(transformation_matrix)
+
+
+    def swap_y_and_z_rotation(self, transform):
+        rotation_quaternion = transform.transform.rotation
+        rotation_matrix = trimesh.transformations.quaternion_matrix([
+            rotation_quaternion.x,
+            rotation_quaternion.y,
+            rotation_quaternion.z,
+            rotation_quaternion.w
+        ])
+        rotation_matrix[[1, 2], :] = rotation_matrix[[2, 1], :]
+        rotation_matrix[:, [1, 2]] = rotation_matrix[:, [2, 1]]
+        new_quaternion = trimesh.transformations.quaternion_from_matrix(rotation_matrix)
+        transform.transform.rotation.x = new_quaternion[0]
+        transform.transform.rotation.y = new_quaternion[1]
+        transform.transform.rotation.z = new_quaternion[2]
+        transform.transform.rotation.w = new_quaternion[3]
+        return transform
+    def swap_x_and_y_rotation(self, transform):
+        rotation_quaternion = transform.transform.rotation
+        rotation_matrix = trimesh.transformations.quaternion_matrix([
+            rotation_quaternion.x,
+            rotation_quaternion.y,
+            rotation_quaternion.z,
+            rotation_quaternion.w
+        ])
+        rotation_matrix[[0, 1], :] = rotation_matrix[[1, 0], :]
+        rotation_matrix[:, [0, 1]] = rotation_matrix[:, [1, 0]]
+        new_quaternion = trimesh.transformations.quaternion_from_matrix(rotation_matrix)
+        transform.transform.rotation.x = new_quaternion[0]
+        transform.transform.rotation.y = new_quaternion[1]
+        transform.transform.rotation.z = new_quaternion[2]
+        transform.transform.rotation.w = new_quaternion[3]
+        return transform
+
+    def swap_x_and_z(self, transform):    
+        rotation_quaternion = transform.transform.rotation
+        rotation_matrix = trimesh.transformations.quaternion_matrix([
+            rotation_quaternion.x,
+            rotation_quaternion.y,
+            rotation_quaternion.z,
+            rotation_quaternion.w
+        ])
+        rotation_matrix[[0, 2], :] = rotation_matrix[[2, 0], :]
+        rotation_matrix[:, [0, 2]] = rotation_matrix[:, [2, 0]]
+        new_quaternion = trimesh.transformations.quaternion_from_matrix(rotation_matrix)
+        transform.transform.rotation.x = new_quaternion[0]
+        transform.transform.rotation.y = new_quaternion[1]
+        transform.transform.rotation.z = new_quaternion[2]
+        transform.transform.rotation.w = new_quaternion[3]
+        return transform
+    
+    def swap_x_and_z_translation(self, transform):
+        temp = transform.transform.translation.x
+        temp = -1 * temp
+        transform.transform.translation.x = transform.transform.translation.z
+        transform.transform.translation.z = temp
+        return transform
+    
+    def invert_y_rotation(self, transform):
+        transform.transform.rotation.y = -transform.transform.rotation.y
+        return transform
+    
+    def get_euler_and_translation(self, transform):
         translation = np.array([transform.transform.translation.x,
                                 transform.transform.translation.y,
                                 transform.transform.translation.z])
@@ -90,10 +204,12 @@ class TransformMeshNode(Node):
                              transform.transform.rotation.y,
                              transform.transform.rotation.z,
                              transform.transform.rotation.w])
-        
-        mesh.apply_transform(trimesh.transformations.quaternion_matrix(rotation))
-        mesh.apply_translation(translation)
-        self.get_logger().info('Mesh transformed successfully')
+        euler_angles = trimesh.transformations.euler_from_quaternion(rotation)
+        print(f'Euler angles: {euler_angles}')
+        print(f'Translation: {translation}')
+    def invert_z_translation(self, transform):
+        transform.transform.translation.z = -transform.transform.translation.z
+        return transform
 
 def main(args=None):
     rclpy.init(args=args)