def __init__(self, geo, tool_plane):
''' tool class for the robot
geo = tool geometry as list
plane = defined tool plane
geo_trfd = tool geometry transformed that the defined tool plane is aligned with the world xy plane
'''
self.geo = geo
self.plane = tool_plane
self.frame = Frame(self.plane, draw_geo=True)
self.T = rg.Transform.PlaneToPlane(self.plane, rg.Plane.WorldXY)
self.geo_WCS = ghcomp.Transform(self.geo,
self.T) # tool aligned_with_WCS
self.plane_WCS = ghcomp.Transform(self.plane,
self.T) # should be WorldXY plane
self.collision_geo = self.geo
self.colors = [
ghcomp.ColourRGB(100, 100, 150, 200)
for i, part in enumerate(self.geo)
]
def __init__(self, geo, gantry_plane):
''' gantry class for the rfl
geo = gantry geometry as list
plane = defined gantry plane - if default - [0,0,0,x,y]
'''
self.geo = geo
self.plane = gantry_plane
self.frame = Frame(self.plane, draw_geo=True)
self.T = rg.Transform.PlaneToPlane(self.plane, rg.Plane.WorldXY)
self.geo_WCS = ghcomp.Transform(self.geo, self.T) # gantry aligned_with_WCS
self.plane_WCS = ghcomp.Transform(self.plane, self.T) # should be WorldXY plane
def get_tool0_pose_from_joint_values_world(self, joint_values):
''' the joint values (in degrees)'''
frame = Frame(
self.get_tool0_plane_from_joint_values_world(joint_values))
return frame.get_pose_quaternion()
def get_tool_plane_from_pose_quaternion(self, pose_quaternion):
''' Returns the tool plane according to the given pose in angle axis. '''
frame = Frame()
frame.set_to_pose_quaternion(pose_quaternion)
return frame.get_plane()
def get_tool_pose_in_RCS(self, tp_WCS):
''' returns the tool pose in the robot coordinate system from a plane in the world cs'''
frame = Frame(self.get_tool_plane_in_RCS(tp_WCS))
return frame.get_pose_angle_axis()
def get_frame(self):
''' returns the tool frame '''
return Frame(self.get_plane())
class Tool(object):
#===========================================================================
def __init__(self, geo, tool_plane):
''' tool class for the robot
geo = tool geometry as list
plane = defined tool plane
geo_trfd = tool geometry transformed that the defined tool plane is aligned with the world xy plane
'''
self.geo = geo
self.plane = tool_plane
self.frame = Frame(self.plane, draw_geo=True)
self.T = rg.Transform.PlaneToPlane(self.plane, rg.Plane.WorldXY)
self.geo_WCS = ghcomp.Transform(self.geo,
self.T) # tool aligned_with_WCS
self.plane_WCS = ghcomp.Transform(self.plane,
self.T) # should be WorldXY plane
self.collision_geo = self.geo
self.colors = [
ghcomp.ColourRGB(100, 100, 150, 200)
for i, part in enumerate(self.geo)
]
#===========================================================================
def set_collision_geo(self, collision_geo):
""" collision geo of the LWS tool"""
self.collison_geo = collision_geo
self.collison_geo_WCS = ghcomp.Transform(self.collison_geo, self.T)
#===========================================================================
def get_plane(self):
''' returns the tool plane '''
return rg.Plane(self.plane)
#===========================================================================
def get_frame(self):
''' returns the tool frame '''
return Frame(self.get_plane())
#===========================================================================
def get_frame_axes_as_lines(self):
''' this method allows you to visualize the tool axes as lines and gh colors '''
tool_axes = self.frame.get_axes_as_lines(length=100)
tool_axes_colors = [
ghcomp.ColourRGB(255, 255, 0, 0),
ghcomp.ColourRGB(255, 0, 255, 0),
ghcomp.ColourRGB(255, 0, 0, 255)
]
return tool_axes, tool_axes_colors
#===========================================================================
def get_frame_axes_as_lines_WCS(self):
''' this method allows you to visualize the tool axes as lines and gh colors transformed into the WCS'''
tool_axes, tool_axes_colors = self.get_frame_axes_as_lines()
tool_axes = ghcomp.Transform(tool_axes, self.T) # aligned_with_WCS
return tool_axes, tool_axes_colors
#===========================================================================
def get_pose_angle_axis(self):
''' returns the pose with the axis angle representation '''
return self.frame.get_pose_angle_axis()
#===========================================================================
def get_transformed_geo(self, T):
''' returns the tool geo transformed by the input transformation matrix '''
return ghcomp.Transform(self.geo, T)
#===========================================================================
def get_transformed_plane(self, T):
''' returns the tool plane transformed by the input transformation matrix '''
return ghcomp.Transform(self.plane, T)
#===========================================================================
def get_transformed_geo_WCS(self, T):
''' returns the tool geo_trfd transformed by the input transformation matrix '''
return ghcomp.Transform(self.geo_WCS, T)