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)
