def get_tool0_frame_from_tcp_frame(self, frame_tcp):
"""Get the tool0 frame (frame at robot) from the tool frame (tcp),
according to the set tool.
"""
T = Transformation.from_frame(frame_tcp)
return Frame.from_transformation(T * self.transformation_tool0_tcp)
def get_transformed_tool_frames(self, R5):
tool0_frame = self.tool0_frame.transform(R5, copy=True)
tcp_frame = Frame.from_transformation(Transformation.from_frame(tool0_frame) * self.transformation_tool0_tcp)
return tool0_frame, tcp_frame