import bezier_interpolation brick_poses = bezier_interpolation.create_path(10, 35, 20, 110, 35, 20) print(brick_poses) for brick in brick_poses: print(brick.x, brick.y, brick.rot)
def main():
"""Simple pick and place example"""
rospy.init_node("right_arm")
# domino.setup_listener_left()
#Change depending on whether running simulation or not
mode_sim = True
# Initialise pick and place
right_pnp = PickAndPlace('right')
left_test = PickAndPlace('left')
domino.safe_point_r(right_pnp)
domino.safe_point_l(left_test)
#Get coordinates from the user
translations, angles = listener.get_coordinates()
start_x = translations[0][0] * 100 + 60
start_y = (translations[0][1] + 0.1) * -100 + 120
start_angle = angles[0][2] - math.pi / 2
end_x = translations[1][0] * 100 + 60
end_y = (translations[1][1] + 0.1) * -100 + 120
end_angle = angles[1][2] + math.pi / 2
print start_x, start_y, ((math.pi) - start_angle)
print end_x, end_y, ((math.pi) + end_angle)
#Get Bezier coordinates for a path
if mode_sim == True:
coords = bezier_interpolation.create_path(10, 35, -3.14 / 8, 110, 40,
3.14 / 8, 110)
else:
coords = bezier_interpolation.create_path(start_x, start_y,
start_angle, end_x, end_y,
end_angle, 110)
#coords = bezier_interpolation.create_path(10,35,20,110,40,10)
def run_pnp(coords):
#Initialise necessary lists
check_list = []
right = []
left = []
#Setup variables
table_height = 0.24
hover = 0.1
#Setup variables for height adjustment
table_length = 160 #160cm
table_reach = table_length / 2 #Each arm only uses half the table
relaive_table_length = 0.6 #Maximum y-distance travelled in gazebo by arm
scale_factor = table_reach / relaive_table_length #Scaling factor
scale_difference = 317.4 #Conversion between gazebo and millimetres
#Table Heights
raised_height = 79 #End with bricks under
lowered_height = 72
height_difference = raised_height - lowered_height
incline_angle = float(math.tan(height_difference / table_length))
print("Incline Angle")
print(float(incline_angle))
#For each brick check inverse kinematics of placement
for brick in coords:
print("brick")
print float(brick.y), float(brick.x), float(brick.rot)
if brick.x <= 0:
right.append(
(float(brick.x), float(brick.y), float(brick.rot)))
else:
left.append((float(brick.x), float(brick.y), float(brick.rot)))
adjusted_z = table_height + (
(float(brick.y) + relaive_table_length) * scale_factor *
incline_angle) / scale_difference
error_check = domino.ik_test(round(brick.y,
3), round(brick.x,
3), adjusted_z,
incline_angle, math.pi, brick.rot,
hover, right_pnp, left_test)
if error_check[0] == False:
check_list.append(error_check[0])
if error_check[1] == False:
check_list.append(error_check[1])
right.sort()
right = right[::-1]
#print('right')
#print(right)
left.sort()
#print('left')
#print(left)
if len(check_list) > 0:
print("Failed Path")
else:
print("Succesful Path")
#Load table model
simulation.load_table()
#Call function to run the left arm simultaneously
rc = subprocess.Popen("python left_placement.py '" + str(left) +
"'",
shell=True)
#Pause to avoid coliision with the left arm
rospy.sleep(10)
#Count how many moves have been made (To load bricks in simulation)
movement_count = 0
for coord in right:
movement_count += 2
adjusted_z = table_height + (
(float(brick.y) + 0.6) * scale_factor *
incline_angle) / scale_difference
print("right")
print coord
domino.pickandplace('r', right_pnp, coord[1], coord[0],
adjusted_z, incline_angle, math.pi,
coord[2] + math.pi / 2, hover,
movement_count)
return check_list
check_list = run_pnp(coords)
#How many paths have we checked?
run_number = 0
influence = 110
#If the first run failed, rerun it
while len(check_list) > 0:
influence -= 10
run_number += 1
print("")
print("Run number {0} failed".format(str(run_number)))
#Rerun path generation with smoother path
#coords = bezier_interpolation.create_path(10,35,0,110,40,0)
if mode_sim == True:
coords = bezier_interpolation.create_path(10, 35, -3.14 / 8, 110,
40, 3.14 / 8, influence)
else:
coords = bezier_interpolation.create_path(start_x, start_y,
start_angle, end_x,
end_y, end_angle,
influence)
#Rerun Ik check for new path
check_list = run_pnp(coords)
#Break after x number of iterations
if run_number >= 10:
break
simulation.delete_gazebo_models()
def main():
"""Simple pick and place example"""
rospy.init_node("ik_pick_and_place_demo")
hover_distance = 0.1 # meters
# Initialise pick and place
left_pnp = PickAndPlace('left', hover_distance)
right_pnp = PickAndPlace('right', hover_distance)
def move(arm, x, y, z, r, p, ya):
#Move to a given point using cartesian coordinates
quat = tf.transformations.quaternion_from_euler(r, p, ya)
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
#print("x = {0}, y = {1}, z = {2}, roll = {3}, pitch = {4}, yaw = {5}".format(x,y,z,r,p,ya))
if arm == 'l':
left_pnp._servo_to_pose(pose)
elif arm == 'r':
right_pnp._servo_to_pose(pose)
return x, y, z, r, p, ya
def slow_move(arm, x, y, z, r, p, ya, steps):
#Function to incrementally move to a point, understand where the motion path fails
dx = (coord[0] - x) / steps
dy = (coord[1] - y) / steps
dz = (coord[2] - z) / steps
dr = (coord[3] - r) / steps
dp = (coord[4] - p) / steps
dya = (coord[5] - ya) / steps
for i in range(steps):
print((x + (steps - (i + 1)) * dx), (y + (steps - (i + 1)) * dy),
(z + (steps - (i + 1)) * dz), (r + (steps - (i + 1)) * dr),
(p + (steps - (i + 1)) * dp), (ya + (steps - (i + 1)) * dya))
move(arm, (x + (steps - (i + 1)) * dx),
(y + (steps - (i + 1)) * dy), (z + (steps - (i + 1)) * dz),
(r + (steps - (i + 1)) * dr), (p + (steps - (i + 1)) * dp),
(ya + (steps - (i + 1)) * dya))
return x, y, z, r, p, ya
def safe_point():
#Function to return arms to tucked position
left_joint_angles = {
'left_s0': -0.5929,
'left_s1': -1.3422,
'left_e0': 0.3146,
'left_e1': 1.3544,
'left_w0': 3.059 - 3.14,
'left_w1': 1.5702,
'left_w2': -1.072
}
right_joint_angles = {
'right_s0': -0.2823,
'right_s1': -1.13965,
'right_e0': 1.0771,
'right_e1': 1.08657,
'right_w0': -0.387,
'right_w1': 1.8194,
'right_w2': -1.7079
}
left_pnp._guarded_move_to_joint_position(left_joint_angles)
right_pnp._guarded_move_to_joint_position(right_joint_angles)
print("Moving to safe point")
def place(x, y, z, r, p, ya, height):
#Function to hover and place bricks on the table
if y > 0:
arm = 'l'
print("Placing with left arm")
elif y <= 0:
arm = 'r'
print("Placing with right arm")
move(arm, x, y, z + height, r, p, ya)
move(arm, x, y, z, r, p, ya)
if arm == 'l':
left_pnp.gripper_open()
rospy.sleep(0.1)
if arm == 'r':
right_pnp.gripper_open()
rospy.sleep(0.1)
move(arm, x, y, z + height, r, p, ya)
#Move back to a safe point
if arm == 'l':
brick_place('l', -0.5929, -1.3422, 0.3146, 1.3544, 3.059 - 3.14,
1.5702, -1.072)
if arm == 'r':
brick_place('r', -0.2823, -1.13965, 1.0771, 1.08657, -0.387,
1.8194, -1.7079)
return x, y, z + height, r, p, ya
def pick(arm):
#Function to pick up a brick with a given arm from a set position
if arm == 'l':
print("Picking with left arm")
#Move to 0.5,0.8,0.5,0,3.14/2,0
brick_place('l', 1.045, -1.2174, -0.5546, 1.8941, 1.5558, -1.2412,
-0.9172)
left_pnp.gripper_open()
coord = move('l', 0.6, 0.8, 0.5, 0, 3.14 / 2, 0)
load_brick2()
left_pnp.gripper_close()
brick_place('l', 1.045, -1.2174, -0.5546, 1.8941, 1.5558, -1.2412,
-0.9172)
# #Move to 0.6,0.5,0.4,0,3.14,-0
#brick_place('l',-0.5967,-1.344,0.3188,1.3571,3.059,-1.5673,-2.642)
#Move to 0.6,0.5,0.4,0,3.14,3.14/2
#brick_place('l',-0.5929,-1.3422,0.3146,1.3544,3.059,-1.5702,-1.072)
brick_place('l', -0.5929, -1.3422, 0.3146, 1.3544, 3.059 - 3.14,
1.5702, -1.072)
elif arm == 'r':
print("Picking with right arm")
#Move tp 0.5,-0.8,0.5,0,3.14/2,0
brick_place('r', -1.032, -1.222, 0.5439, 1.897, -1.5479, -1.239,
0.9162)
right_pnp.gripper_open()
coord = move('r', 0.6, -0.8, 0.5, 0, 3.14 / 2, 0)
#load_brick1()
right_pnp.gripper_close()
brick_place('r', -1.032, -1.222, 0.5439, 1.897, -1.5479, -1.239,
0.9162)
#Move to 0.6,-0.5,0.45,0,3.14,-0
brick_place('r', -0.1652, -1.2395, 0.81048, 1.1156, -0.2439,
1.7843, -0.222)
#Move to 0.6,-0.5,0.45,0,3.14,3.14/2
brick_place('r', -0.2823, -1.13965, 1.0771, 1.08657, -0.387,
1.8194, -1.7079)
def pickandplace(x, y, z, r, p, ya, height):
#Combined pick and place functions
if y > 0:
arm = 'l'
elif y <= 0:
arm = 'r'
pick(arm)
place(x, y, z, r, p, ya, height)
def brick_place(arm, s0, s1, e0, e1, w0, w1, w2):
#Function to use joint angles to move to point
if arm == 'l':
joint_angles = {
'left_s0': s0,
'left_s1': s1,
'left_e0': e0,
'left_e1': e1,
'left_w0': w0,
'left_w1': w1,
'left_w2': w2
}
left_pnp._guarded_move_to_joint_position(joint_angles)
elif arm == 'r':
joint_angles = {
'right_s0': s0,
'right_s1': s1,
'right_e0': e0,
'right_e1': e1,
'right_w0': w0,
'right_w1': w1,
'right_w2': w2
}
right_pnp._guarded_move_to_joint_position(joint_angles)
def knock_down(x, y, z):
#Function to knock down the arranged bricks
if y >= 0:
print("Knocking down with left arm")
arm = 'l'
offset = 0.1
brick_place('l', -0.5929, -1.3422, 0.3146, 1.3544, 3.059 - 3.14,
1.5702, -1.072)
left_pnp.gripper_close()
elif y <= 0:
print("Knocking down with right arm")
arm = 'r'
offset = -0.1
brick_place('r', -0.2823, -1.13965, 1.0771, 1.08657, -0.387,
1.8194, -1.7079)
right_pnp.gripper_close()
coord = move(arm, x, y + offset, z, 0, 3.14, 0)
coord = move(arm, x, y - offset, z, 0, 3.14, 0)
def ik_test(x, y, z, r, p, ya):
#Function to test whether a range of coordinates are within workspace
quat = tf.transformations.quaternion_from_euler(r, p, ya)
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose2 = Pose()
pose2.position.x = x
pose2.position.y = y
pose2.position.z = z + 0.15
pose2.orientation.x = quat[0]
pose2.orientation.y = quat[1]
pose2.orientation.z = quat[2]
pose2.orientation.w = quat[3]
if y <= 0:
limb_joints = right_pnp.ik_request(pose)
limb_joints_up = right_pnp.ik_request(pose2)
if limb_joints == False or limb_joints_up == False:
print("Right arm failed")
limb_joints = left_pnp.ik_request(pose)
limb_joints_up = left_pnp.ik_request(pose2)
if limb_joints_up == False or limb_joints_up == False:
print("Try 2 - left failed")
print("FAILED COORDINATE")
else:
print("SUCCESSFUL COORDINATE")
pass
else:
print("SUCCESSFUL COORDINATE")
pass
elif y >= 0:
limb_joints = left_pnp.ik_request(pose)
limb_joints_up = left_pnp.ik_request(pose2)
if limb_joints == False or limb_joints_up == False:
print("Left arm failed")
limb_joints = right_pnp.ik_request(pose)
limb_joints_up = right_pnp.ik_request(pose2)
if limb_joints == False or limb_joints_up == False:
print("Try 2 - right failed")
print("FAILED COORDINATE")
else:
print("SUCCESSFUL COORDINATE")
pass
else:
print("SUCCESSFUL COORDINATE")
pass
return limb_joints, limb_joints_up
safe_point()
#Get coordinates from the user
translations, angles = listener.get_coordinates()
start_x = translations[0][0] * 100 + 60
start_y = (translations[0][1] + 0.1) * -100 + 120
start_angle = angles[0][2] - 3.1415 / 2
end_x = translations[1][0] * 100 + 60
end_y = (translations[1][1] + 0.1) * -100 + 120
end_angle = angles[1][2] - 3.1415 / 2
print start_x, start_y, ((3.14 / 2) + start_angle)
print end_x, end_y, ((3.14 / 2) + end_angle)
#Get Bezier coordinates
#coords = bezier_interpolation.create_path(start_x, start_y, start_angle, end_x, end_y, end_angle)
coords = bezier_interpolation.create_path(10, 35, 20, 110, 40, 10)
check_list = []
right = []
left = []
for brick in coords:
print("brick")
print float(brick.y), float(brick.x), float(brick.rot)
if brick.y <= 0:
right.append((float(brick.y), float(brick.x), float(brick.rot)))
else:
left.append((float(brick.y), float(brick.x), float(brick.rot)))
error_check = ik_test(round(brick.y, 3), round(brick.x, 3), 0.23, 0,
3.14, brick.rot)
if error_check[0] == False:
check_list.append(error_check[0])
if error_check[1] == False:
check_list.append(error_check[1])
right.sort()
right = right[::-1]
#print('right')
#print(right)
left.sort()
#print('left')
#print(left)
ordered_coords = left + right
#print('coord\n' + str(coords))
#print('ordered\n' + str(ordered_coords))
#Load models
load_table()
if len(check_list) > 0:
print("Failed Path")
else:
for brick in ordered_coords:
print(brick[1], brick[0])
pickandplace(brick[1], brick[0], 0.23, 0, 3.14, brick[2], 0.15)
#safe_point()
#Test IK conditions
#ik_test(0.8,-0.1,0.25,0,3.14/2,3.14/2)
#coord = move('r',0.8,-0.1,0.25,0,3.14/2,3.14/2)
#Pick up bricks
# pick('r')
# pick('l')
#Robust algorithm for picking and placing bricks in a straight line
# pickandplace(0.7,0.0,0.25,0,3.14,3.14/2)
# pickandplace(0.7,-0.1,0.25,0,3.14,3.14/2)
# pickandplace(0.7,0.1,0.25,0,3.14,3.14/2)
# pickandplace(0.7,-0.2,0.25,0,3.14,3.14/2)
# pickandplace(0.7,0.2,0.25,0,3.14,3.14/2)
# pickandplace(0.7,-0.3,0.25,0,3.14,3.14/2)
# pickandplace(0.7,0.3,0.25,0,3.14,3.14/2)
# pickandplace(0.7,-0.4,0.25,0,3.14,3.14/2)
# pickandplace(0.7,0.4,0.25,0,3.14,3.14/2)
# knock_down(0.7,-0.5,0.23)
#Robust algorithm for picking and placing bricks in a straight line
# pickandplace(0.7,0.0,0.25,0,3.14,3.14/2)
# pickandplace(0.71,-0.1,0.25,0,3.14,3.14/2+0.2)
# pickandplace(0.7,0.1,0.25,0,3.14,3.14/2)
# pickandplace(0.70,-0.2,0.25,0,3.14,3.14/2+0.2)
# pickandplace(0.7,0.2,0.25,0,3.14,3.14/2)
# pickandplace(0.73,-0.3,0.25,0,3.14,3.14/2+0.2)
delete_gazebo_models()
def main():
rospy.init_node(
"right_arm") #Initiate a node to control computation for the right arm
mode_sim = True #Change depending on whether running simulation or not
#Initialise an instance of the arm class for both arms
right_pnp = arm('right')
left_test = arm('left')
#Move both arms to a midpoint using joint angles
domino.safe_point_r(right_pnp)
domino.safe_point_l(left_test)
#Get coordinates from the user by creating
translations, angles = listener.get_coordinates()
start_x = translations[0][0] * 100 + 60
start_y = (translations[0][1] + 0.1) * -100 + 120
start_angle = angles[0][2] - math.pi / 2
end_x = translations[1][0] * 100 + 60
end_y = (translations[1][1] + 0.1) * -100 + 120
end_angle = angles[1][2] + math.pi / 2
print start_x, start_y, ((math.pi) - start_angle)
print end_x, end_y, ((math.pi) + end_angle)
#Get Bezier coordinates for a path
if mode_sim == True:
coords = bezier_interpolation.create_path(10, 35, -math.pi / 8, 110,
40, math.pi / 8, 110)
else:
coords = bezier_interpolation.create_path(start_x, start_y,
start_angle, end_x, end_y,
end_angle, 110)
def run_pnp(coords):
check_list = [] #For checking whether a path succeeds
right = [] #Coordinates for right arm
left = [] #Coordinates for left arm
#Setup variables
table_height = 0.24
hover = 0.1
#Setup variables for height adjustment
table_length = 160 #160cm
table_reach = table_length / 2 #Each arm only uses half the table
relaive_table_length = 0.6 #Maximum y-distance travelled in gazebo by arm
scale_factor = table_reach / relaive_table_length #Scaling factor
scale_difference = 317.4 #Conversion between gazebo and millimetres
raised_height = 79 #Table height in mm (raised side)
lowered_height = 72
height_difference = raised_height - lowered_height
incline_angle = float(math.tan(height_difference / table_length))
#For each brick check inverse kinematics of placement
for brick in coords:
print("brick")
print float(brick.y), float(brick.x), float(brick.rot)
if brick.x <= 0: #Split coordinates based on whether they are on the right or left side
right.append(
(float(brick.x), float(brick.y), float(brick.rot)))
else:
left.append((float(brick.x), float(brick.y), float(brick.rot)))
#Adjust coordinates to account for slant of table
adjusted_z = table_height + (
(float(brick.y) + relaive_table_length) * scale_factor *
incline_angle) / scale_difference
#Check inverse kinematics for each point on a path
error_check = domino.ik_test(round(brick.y,
3), round(brick.x,
3), adjusted_z,
incline_angle, math.pi, brick.rot,
hover, right_pnp, left_test)
#Flag if any of the coordinates on the path fail
if error_check[0] == False:
check_list.append(error_check[0])
if error_check[1] == False:
check_list.append(error_check[1])
if len(check_list) > 0:
print("Failed Path")
else:
print("Succesful Path")
#Order the coordinates
right.sort()
right = right[::-1]
left.sort()
#Load table model
if mode_sim == True:
simulation.load_table()
#Call function to run the left arm simultaneously, feed the succesful coordinates to the subprocess by inputting them as command line arguments
rc = subprocess.Popen("python left_placement.py '" + str(left) +
"'",
shell=True)
rospy.sleep(10) #Pause to avoid coliision with the left arm
movement_count = 0 #Count how many moves have been made (To load bricks in simulation)
for coord in right:
movement_count += 2
#Call function to pick and place bricks at a specific coordinate
adjusted_z = table_height + (
(float(brick.y) + 0.6) * scale_factor *
incline_angle) / scale_difference
domino.pickandplace('r', right_pnp, coord[1], coord[0],
adjusted_z, incline_angle, math.pi,
coord[2] + math.pi / 2, hover,
movement_count, mode_sim)
return check_list
check_list = run_pnp(coords) #Check the first path
run_number = 0 #How many paths have been checked
influence = 110 #Handle influence (affects curvature of path)
#If the first run failed, rerun
while len(check_list) > 0:
influence -= 10 #Decrease to straighten path
run_number += 1
print("")
print("Run number {0} failed".format(str(run_number)))
#Create a new path
if mode_sim == True:
coords = bezier_interpolation.create_path(10, 35, -3.14 / 8, 110,
40, 3.14 / 8, influence)
else:
coords = bezier_interpolation.create_path(start_x, start_y,
start_angle, end_x,
end_y, end_angle,
influence)
#Rerun with the new, straighter path
check_list = run_pnp(coords)
#After 10 iterations the path is straight, break if it still doesn't work
if run_number >= 10:
break
if mode_sim == True:
simulation.delete_gazebo_models()