diff --git a/workspaces/COLCON_WS/src/ser_test/ser_test/test.py b/workspaces/COLCON_WS/src/ser_test/ser_test/test.py
index 0258204afc2f8a41f21a8dfad2ac6f92feffaeba..e6e09e0406e1f165c7f0ff12bde4677711833c39 100644
--- a/workspaces/COLCON_WS/src/ser_test/ser_test/test.py
+++ b/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)
diff --git a/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl
index 4d6631c75cde8d3ee7915c445b920032e5f15864..60fa0d10063a999a39b69f60b3634ecaacbcfeb2 100644
Binary files a/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl and b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl differ
diff --git a/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/shoulder.stl b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/shoulder.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f3b1e7cf6ae8c83bca25797db88c10132d3a8d84
Binary files /dev/null and b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/shoulder.stl differ
diff --git a/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/upperarm.stl b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/upperarm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..5c958dadc81a397047e1c4896dc4c5831fbad386
Binary files /dev/null and b/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision_custom/upperarm.stl differ