def coffeeMachineButtons(): T_GT_CMBase_np = np.matmul(T_TCP_CMBase_np, T_TCP_GT_np) P_CM_B1Set_np = np.array([70, 98.75, -32]) #B1 set position P_CM_B1Set1_np = np.array([70, 0.00, -32]) P_CM_B1Push_np = np.array([50, 98.75, -32]) T_CM_B1Set_np = np.zeros((4, 4)) T_CM_B1Set1_np = np.zeros((4, 4)) T_CM_B1Push_np = np.zeros((4, 4)) T_CM_B1Set_np[3, 3] = 1.0 T_CM_B1Set_np[0:3, 0:3] = R_CM_B_np T_CM_B1Set1_np[3, 3] = 1.0 T_CM_B1Set1_np[0:3, 0:3] = R_CM_B_np T_CM_B1Push_np[3, 3] = 1.0 T_CM_B1Push_np[0:3, 0:3] = R_CM_B_np T_CM_B1Set_np[0:3, 3] = P_CM_B1Set_np T_CM_B1Set1_np[0:3, 3] = P_CM_B1Set1_np T_CM_B1Push_np[0:3, 3] = P_CM_B1Push_np T_GT_B1Set_np = np.matmul(np.matmul(T_TCP_CMBase_np, T_CM_B1Set_np), T_TCP_GTP_np) T_GT_B1Set1_np = np.matmul(np.matmul(T_TCP_CMBase_np, T_CM_B1Set1_np), T_TCP_GTP_np) T_GT_B1Push_np = np.matmul(np.matmul(T_TCP_CMBase_np, T_CM_B1Push_np), T_TCP_GTP_np) T_GT_CMBase = rdk.Mat(T_GT_CMBase_np.tolist()) T_GT_B1Set = rdk.Mat(T_GT_B1Set_np.tolist()) T_GT_B1Set1 = rdk.Mat(T_GT_B1Set_np.tolist()) T_GT_B1Push = rdk.Mat(T_GT_B1Push_np.tolist()) #Movement to press B1 from home robot.MoveJ(J_Home, blocking=True) robot.MoveJ(J_intermediateGrinderTool, blocking=True) RDK.RunProgram('Grinder Tool Attach (Stand)', True) J_GT_B1Set1 = [ -153.922130, -111.554777, -111.667569, 43.222346, 168.960130, -130.000000 ] robot.MoveL(J_GT_B1Set1, blocking=True) robot.MoveL(T_GT_B1Set, blocking=True) robot.MoveL(T_GT_B1Push, blocking=True) robot.MoveJ(T_GT_B1Set, blocking=True) robot.MoveJ(J_intermediateGrinderTool, blocking=True) RDK.RunProgram('Grinder Tool Detach (Stand)', True)
def move_pen_to(self, x, y, z): self.robot.setPoseFrame(self.draw_frame) self.robot.setPoseTool(self.draw_tool) orient_frame2tool = self.draw_tool.Pose() orient_frame2tool[0:3, 3] = rdk.Mat([0, 0, 0]) pos = rdk.transl(x, y, z) * orient_frame2tool self.robot.MoveL(pos)
def read_joint_angles(filename): file = open(filename, "r") lines = file.readlines() file.close() joint_angles = {} for line in lines: segments = line.rstrip().split(",") name = segments[0] values = [float(i) for i in segments[1:]] angles = rdk.Mat(values) joint_angles[name] = angles return joint_angles
def main(): filename = 'reference_frames.csv' frames = read_frames(filename) all_true = True for name, frame in frames.items(): print(name) print(frame) print("{} is homogenous: {}".format(name, frame.isHomogeneous())) if not frame.isHomogeneous(): all_true = False print(all_true) silvia_frame_point = rdk.Mat([0, 218, 0, 1]) silvia_robot_point = frames[GLOBAL + SILVIA] * silvia_frame_point grinder_frame_point = rdk.Mat([157.61, 0, -250.45, 1]) grinder_robot_point = frames[GLOBAL + GRINDER] * grinder_frame_point cross_frame_point = rdk.Mat([-80, 0, -55, 1]) cross_robot_point = frames[GLOBAL + CROSS] * cross_frame_point test_points = [silvia_robot_point, grinder_robot_point, cross_robot_point] plot_scene(frames, test_points)
def portafilterPlacement(): #Calculate placement position for portafilter under the grinder T_TCP_PFP_np = np.matmul(T_TCP_GR_np, np.matmul(T_GR_PFP_np, T_CL_PFT_np)) T_TCP_PFP = rdk.Mat(T_TCP_PFP_np.tolist()) T_TCP_GR = rdk.Mat(T_TCP_GR_np.tolist()) #Steps from tools tand to Grinder J_Step1 = [ -156.440000, -81.180000, -75.390000, -181.780000, 181.780000, -182.170000 ] J_Step2 = [ -156.440000, -81.180000, -127.130000, -181.780000, 181.780000, -182.170000 ] J_Step3 = [ -71.050000, -81.180000, -127.130000, -136.630000, 181.780000, -182.170000 ] J_Step4 = [ -14.210000, -65.350000, -148.510000, -148.510000, 293.470000, -231.680000 ] J_Step5 = [ -17.818290, -100.202192, -139.887909, -111.506940, 296.722534, -223.800304 ] robot.MoveJ(J_Home, blocking=True) robot.MoveJ(J_intermediatePortafilter, blocking=True) RDK.RunProgram('Portafilter Tool Attach (Stand)', True) robot.MoveJ(J_Step1, blocking=True) robot.MoveJ(J_Step2, blocking=True) robot.MoveJ(J_Step3, blocking=True) robot.MoveJ(J_Step4, blocking=True) robot.MoveJ(J_Step5, blocking=True) #robot.MoveL(T_TCP_PFP, blocking=True) RDK.RunProgram('Portafilter Tool Detach (Grinder)', True)
def read_frames(filename): file = open(filename, "r") lines = file.readlines() file.close() frames = {} for line in lines: segments = line.rstrip().split(",") name = segments[0] values = [float(i) for i in segments[1:]] transform = rdk.Mat( [values[0:4], values[4:8], values[8:12], values[12:16]]) frames[name] = transform return frames
def read_joint_angles(filename): """ Read joint angles from CSV file and convert into dictionary of RoboDK matrices. filename: file path to CSV file output: dictionary of joint angles where key describing the final pose """ # Open file file = open(filename, "r") lines = file.readlines() file.close() # Read each line of file and convert into RoboDK matrix joint_angles = {} for line in lines: segments = line.rstrip().split(",") name = segments[0] values = [float(i) for i in segments[1:]] angles = rdk.Mat(values) joint_angles[name] = angles return joint_angles
def pullLever(): #Calculate the position of the lever T_TCP_GTL_np = np.matmul( np.matmul(np.matmul(T_TCP_GR_np, T_GR_GL_np), T_GT_GTL_np), T_TCP_GT_np) T_TCP_GTL = rdk.Mat(T_TCP_GTL_np.tolist()) J_TCP_GTLPreset = [ 107.180244, -60.445471, 91.787676, -31.342205, 51.970244, -130.000000 ] J_TCP_GTLSet = [ 108.678666, -65.599414, 99.954969, -34.355555, 53.468666, -130.000000 ] J_TCP_GTLPulled = [ 104.942740, -70.827352, 107.768210, -36.940858, 49.732740, -130.000000 ] #robot.MoveJ(T_TCP_GTL, blocking=True) robot.MoveJ(J_TCP_GTLPreset, blocking=True) robot.MoveL(J_TCP_GTLSet, blocking=True) robot.MoveL(J_TCP_GTLPulled, blocking=True) RDK.RunProgram('Grinder Tool Detach (Stand)', True)
def read_frames(filename): """ Read transformation matrices from CSV file and convert into dictionary of RoboDK matrices. filename: file path to CSV file output: dictionary of transformation matrices where key is a string composed of the first frame and second frame titles """ # Open file file = open(filename, "r") lines = file.readlines() file.close() # Read each line of file and convert into RoboDK matrix frames = {} for line in lines: segments = line.rstrip().split(",") name = segments[0] values = [float(i) for i in segments[1:]] # CSV file is formatted as each row sequentially separated by ',' transform = rdk.Mat([values[0:4], values[4:8], values[8:12], values[12:16]]) frames[name] = transform return frames
theta_grinder = np.radians(134.86) theta_press = np.radians(59.73) # Define the base transforms. T_coffee_machine_base_np = np.array( [[ np.cos(theta_coffee_machine), -np.sin(theta_coffee_machine), 0.000000, -366.200000 ], [ np.sin(theta_coffee_machine), np.cos(theta_coffee_machine), 0.000000, -389.800000 ], [0.000000, 0.000000, 1.000000, 341.380000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_coffee_machine_base = rdk.Mat(T_coffee_machine_base_np.tolist()) T_grinder_base_np = np.array( [[np.cos(theta_grinder), -np.sin(theta_grinder), 0.000000, 482.290000], [np.sin(theta_grinder), np.cos(theta_grinder), 0.000000, -433.740000], [0.000000, 0.000000, 1.000000, 314.130000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_grinder_base = rdk.Mat(T_grinder_base_np.tolist()) T_press_base_np = np.array( [[np.cos(theta_press), -np.sin(theta_press), 0.000000, 599.130000], [np.sin(theta_press), np.cos(theta_press), 0.000000, 0.000000], [0.000000, 0.000000, 1.000000, 156.070000],
def pushButtonsOnGrinder(): #Steps to go from coffee grinder to toolstand J_GR_TS_Step1 = [ -10.984030, -94.722451, -149.513590, -106.760819, 303.478379, -224.994922 ] J_GR_TS_Step2 = [ -42.630000, -94.720000, -149.510000, -106.760000, 303.470000, -224.990000 ] J_GR_TS_Step3 = [ -61.580000, -94.720000, -41.580000, -106.760000, 360.000000, -224.990000 ] J_GR_TS_Step4 = [ -118.420000, -94.720000, -41.580000, -106.760000, 84.360000, -224.990000 ] robot.MoveJ(J_GR_TS_Step1, blocking=True) robot.MoveJ(J_GR_TS_Step2, blocking=True) robot.MoveJ(J_GR_TS_Step3, blocking=True) robot.MoveJ(J_GR_TS_Step4, blocking=True) #go and grab the Grinder tool #robot.MoveJ(J_intermediateGrinderTool, blocking=True) RDK.RunProgram('Grinder Tool Attach (Stand)', True) #Steps to go from the tool stand to the coffee grinder J_TS_GR_Step1 = [ -78.160000, -93.120000, -64.110000, -112.910000, 89.940000, -167.400000 ] J_TS_GR_Step2 = [ -11.840000, -93.120000, -64.110000, -112.910000, 89.940000, -167.400000 ] J_TS_GR_Step3 = [ 11.840000, -93.120000, 67.720000, -112.910000, 89.940000, -167.400000 ] J_TS_GR_Step4 = [ 80.530000, -93.120000, 112.870000, -112.910000, 89.940000, -167.400000 ] J_TS_GR_Step5 = [ 87.630000, -81.980000, 112.860000, -112.910000, 3.560000, -167.400000 ] robot.MoveJ(J_TS_GR_Step1, blocking=True) robot.MoveJ(J_TS_GR_Step2, blocking=True) robot.MoveJ(J_TS_GR_Step3, blocking=True) robot.MoveJ(J_TS_GR_Step4, blocking=True) robot.MoveJ(J_TS_GR_Step5, blocking=True) #Calculate position of button T_TCP_GB_np = np.matmul(np.matmul(T_TCP_GR_np, T_GR_GB_np), T_TCP_GTP_np) T_TCP_GB = rdk.Mat(T_TCP_GB_np.tolist()) J_TCP_GB1Preset = [ 101.294020, -35.722756, 59.343965, -23.621209, -18.665980, -130.000000 ] J_TCP_GB1PushB1 = [ 103.143892, -36.255822, 60.387863, -24.132041, -16.816108, -130.000000 ] J_TCP_GB2Preset = [ 101.723526, -33.217697, 54.419557, -21.201859, -18.236474, -130.000000 ] J_TCP_GB2PushB1 = [ 103.534098, -33.777215, 55.522078, -21.744863, -16.425902, -130.000000 ] robot.MoveJ(J_TCP_GB1Preset, blocking=True) robot.MoveL(J_TCP_GB1PushB1, blocking=True) robot.MoveJ(J_TCP_GB2Preset, blocking=True) robot.MoveL(J_TCP_GB2PushB1, blocking=True) robot.MoveL(T_TCP_GB, blocking=True) #This is the origin of plane
# RoboDK API import robodk as rdk import robolink as rl # Robot toolbox import numpy as np RDK = rl.Robolink() robot = RDK.Item('UR5') world_frame = RDK.Item('UR5 Base') target = RDK.Item('Home') # existing target in station robot.setPoseFrame(world_frame) robot.setPoseTool(robot.PoseTool()) # Directly use the RDK Matrix object from to hold pose (its an HT) T_home = rdk.Mat([[0.000000, 0.000000, 1.000000, 523.370000], [-1.000000, 0.000000, 0.000000, -109.000000], [-0.000000, -1.000000, 0.000000, 607.850000], [0.000000, 0.000000, 0.000000, 1.000000]]) J_Home = [ -211.045963, -83.359584, 54.376727, 28.982857, 58.954037, -180.000000 ] #to help keep home position constant # Joint angles J_intermediateGrinderTool = [ 13.072486, -86.867471, 64.139943, -67.272472, 270.000248, -283.072495 ] J_intermediateCupTool = [ -183.954225, -98.097022, -52.302160, -119.399545, 90.411813, -193.952774 ] J_intermediatePortafilter = [ -156.440056, -81.180555, -75.398392, -113.543732, 89.907041, -182.170785
[0.000000, 0.000000, 0.000000, 0.000000]]) # Add in offsets in the cup frame. Align_cup_np = np.matmul(cup_frame_np, T_cup_angle_np + cup_offset_np) Grabber_cup_centre_np = np.matmul(cup_frame_np, T_cup_angle_np + cup_centre_np) Grabber_above_cup_centre_np = np.matmul(cup_frame_np, T_cup_angle_np + above_cup_centre_np) Align_coffee_machine_np = np.matmul(coffee_machine_frame_np, T_cup_grabber_angle_np + coffee_machine_offset_np) Center_coffee_machine_np = np.matmul(coffee_machine_frame_np, T_cup_grabber_angle_np + coffee_machine_center_np) ################################################################################################### """ NP to rdk.Mat conversions. """ ################################################################################################### # Convert to rdk.Mat format. T_cup_base = rdk.Mat(T_cup_base_np.tolist()) T_cup_grabber_frame = rdk.Mat(T_cup_grabber_frame_np.tolist()) cup_frame = rdk.Mat(cup_frame_np.tolist()) T_cup_angle = rdk.Mat(T_cup_angle_np.tolist()) oriented_frame = rdk.Mat(oriented_frame_np.tolist()) Align_cup = rdk.Mat(Align_cup_np.tolist()) Grabber_cup_centre = rdk.Mat(Grabber_cup_centre_np.tolist()) coffee_machine_frame = rdk.Mat(coffee_machine_frame_np.tolist())
RDK = rl.Robolink() robot = RDK.Item('UR5') world_frame = RDK.Item('UR5 Base') target = RDK.Item('Home') # existing target in station robot.setPoseFrame(world_frame) robot.setPoseTool(robot.PoseTool()) #Existing subprograms grinder_tool_attach = RDK.Item('Grinder Tool Attach') grinder_tool_detach = RDK.Item('Grinder Tool Detach') # Directly use the RDK Matrix object from to hold pose (its an HT) T_home = rdk.Mat([[ 0.000000, 0.000000, 1.000000, 523.370000 ], [-1.000000, 0.000000, 0.000000, -109.000000 ], [-0.000000, -1.000000, 0.000000, 607.850000 ], [0.000000, 0.000000, 0.000000, 1.000000 ]]) # Joint angles J_intermediatePointPusher = [-151.880896, -97.616411, -59.103383, -112.890980, 90.242082, -161.879346] J_intermediateCupTool = [-183.954225, -98.097022, -52.302160, -119.399545, 90.411813, -193.952774] T_intermediatePortafilter_np = np.array([[ 0,-1, 0, -399.59], [-1, 0, 0, -55.10], [ 0, 0,-1, 534.02], [ 0, 0, 0, 1.00]]) # Convert a numpy array into a Mat (e.g.after calculation) T_grinderToolApproach_np = np.array([[ 0.173648, -0.984800, -0.004000, -502.103741], [ -0.984789, -0.173618, -0.006928, -145.353888 ], [ 0.006128, 0.005142, -0.999968, 535.250260 ], [ 0.000000, 0.000000, 0.000000, 1.000000 ]])
RDK = rl.Robolink() robot = RDK.Item('UR5') world_frame = RDK.Item('UR5 Base') target = RDK.Item('Home') # existing target in station robot.setPoseFrame(world_frame) robot.setPoseTool(robot.PoseTool()) #Existing subprograms grinder_tool_attach = RDK.Item('Grinder Tool Attach') grinder_tool_detach = RDK.Item('Grinder Tool Detach') # Use the RDK Matrix object from to hold pose (its an HT) T_home = rdk.Mat([[0.000000, 0.000000, 1.000000, 523.370000], [-1.000000, 0.000000, 0.000000, -109.000000], [-0.000000, -1.000000, 0.000000, 607.850000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_intermediatepoint = rdk.Mat([[-0.642784, -0.766037, -0.004005, -501.690000], [-0.766007, 0.642795, -0.006929, -144.638000], [0.007883, -0.001386, -0.999968, 498.577000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_grinderapproach = rdk.Mat([[0.173648, -0.984800, -0.004000, -502.103741], [-0.984789, -0.173618, -0.006928, -145.353888], [0.006128, 0.005142, -0.999968, 535.250260], [0.000000, 0.000000, 0.000000, 1.000000]]) robot.MoveJ(T_home, blocking=True) robot.MoveJ(T_intermediatepoint, blocking=True) robot.MoveL(T_grinderapproach, blocking=True)
# Else set up simulation else: print('Running simulation') RDK.setRunMode(rl.RUNMODE_SIMULATE) # Set robot to scan position and set up other settings scanStartPos = RDK.Item('Scanstart') robotHomePos = RDK.Item('Home') robot.setPoseFrame(robot.PoseFrame()) robot.setPoseTool(RDK.Item('Scanner TCP')) robot.setSpeed(50) robot.setAcceleration(50) robot.MoveL(scanStartPos) robot.setSpeed(50) scanPose = rd.Mat(robot.Pose()) # Connect to lidar scan = ls.Lidar(lidarIP) scan.connect() if debug: print('LiDAR Connected') # Create a unique filename sd = os.path.dirname(__file__) path = sd + '/Scans/' filename = ('scan_' + str(datetime.datetime.now().date()) + '_' + str(datetime.datetime.now().time()).replace(':', '_')) filename = filename + '.txt' filewrite = path + filename # Open file for writing with open(filewrite, 'w') as fn:
target = RDK.Item('Home') # Existing target in station robot.setPoseFrame(world_frame) robot.setPoseTool(robot.PoseTool()) T_TCP_cup_base_np = np.array([[-1.000000, 0.000000, 0.000000, 1.490000], [0.000000, -1.000000, 0.000000, -600.540000], [0.000000, 0.000000, 1.000000, -20.000000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_cup_base_grabberFrame_np = np.array( [[1.000000, 0.000000, 0.000000, 0.0000], [0.000000, 0.000000, 1.000000, 0.0000], [0.000000, -1.000000, 0.000000, 0.000000], [0.000000, 0.000000, 0.000000, 1.000000]]) T_cup_base_grabberFrame = rdk.Mat(T_cup_base_grabberFrame_np.tolist()) theta_TCP_CT = np.radians(40) T_cup_angle_np = np.array( [[np.cos(theta_TCP_CT), np.sin(theta_TCP_CT), 0.000000, 00.000000], [-np.sin(theta_TCP_CT), np.cos(theta_TCP_CT), 0.0000, -47.000000], [0.000000, 0.000000, 1.000000, -186.11], [0.000000, 0.000000, 0.000000, 1.000000]]) T_TCP_GrabberFrame_np = np.matmul( np.matmul(T_TCP_cup_base_np, T_cup_base_grabberFrame_np), T_cup_angle_np) T_TCP_cup_base = rdk.Mat(T_TCP_cup_base_np.tolist()) T_TCP_GrabberFrame = rdk.Mat(T_TCP_GrabberFrame_np.tolist()) #RDK.RunProgram("Cup Tool Detach (Stand)", True)
RDK = rl.Robolink() robot = RDK.Item('UR5') world_frame = RDK.Item('UR5 Base') target = RDK.Item('Home') # existing target in station robot.setPoseFrame(world_frame) robot.setPoseTool(robot.PoseTool()) # Existing subprograms grinder_tool_attach = RDK.Item('Grinder Tool Attach') grinder_tool_detach = RDK.Item('Grinder Tool Detach') # Directly use the RDK Matrix object from to hold pose (its an HT) T_home = rdk.Mat([[0.000000, 0.000000, 1.000000, 523.370000], [-1.000000, 0.000000, 0.000000, -109.000000], [-0.000000, -1.000000, 0.000000, 607.850000], [0.000000, 0.000000, 0.000000, 1.000000]]) # Joint angles J_intermediatepoint = [-151.880896, -97.616411, -59.103383, -112.890980, 90.242082, -161.879346] # Convert a numpy array into a Mat (e.g.after calculation) T_grinderapproach_np = np.array([[0.173648, -0.984800, -0.004000, -502.103741], [-0.984789, -0.173618, -0.006928, -145.353888], [0.006128, 0.005142, -0.999968, 535.250260], [0.000000, 0.000000, 0.000000, 1.000000]]) T_grinderapproach = rdk.Mat(T_grinderapproach_np.tolist()) robot.MoveJ(T_home, blocking=True) robot.MoveJ(J_intermediatepoint, blocking=True)