Software apps and online services | ||||||
 |
| |||||
.png?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff) |
| |||||
The goal of this project was to build a leaning bridge of Lire that is at least 6 blocks high using the UR3 robot arm to pick and place the sticks.
An OpenCV camera was used for blob detection and to locate the position and orientation of each stick. Then the joint angles required for the robot arm to move to the desired locations to pick up and place the sticks were calculated using inverse kinematics.
Setup of this project:It is required that all the sticks of the structure have the same orientation, that is, all yellow blocks should face the same direction and all green blocks should face the opposite direction. Therefore, the yaw of the end-effector needs to be calculated. The yaw is defined as the angle between the x axis in the world frame and the end-effector carrying the suction cup. When picking the sticks, this yaw is aligned with the orientation of the sticks, which is defined as the angle between the world's x axis and the imaginary vector pointing from the purple block to the yellow block.
Methods:The blob detection function would return a tuple of coordinates of yellow and purple blocks in an arbitrary order, which means there is no guarantee that the yellow block and the purple block come from the same stick if these two blocks are paired based on their index in the tuple.
A for loop was used to pair each yellow block with the purple block that is the closest, each of these pairs indicate that the two blocks are in the same stick. Then the orientation of the stick is calculated using inverse tangent based on the difference between the x and y coordinates of the paired yellow and purple blocks. These angles are also the yaw values required when picking up the sticks. The example code below shows the grouping of the blocks and the calculation of yaw values.
list_yaw = [0]*11
dist = [0]*11 # list of distance between a yellow block with all purple blocks
for i in range(0,len(xw_yw_Y)):
for j in range(0,len(xw_yw_P)):
dist[j] = np.sqrt((xw_yw_Y[i][0]-xw_yw_P[j][0])**2 + (xw_yw_Y[i][1]-xw_yw_P[j][1])**2)
min_dist = min(dist) # store the minimum distance
min_index = dist.index(min_dist) # index of the minimum distance (corresponding purple block to pick)
delta_x = xw_yw_Y[i][0]-xw_yw_P[min_index][0]
delta_y = xw_yw_Y[i][1]-xw_yw_P[min_index][1]
yaw = np.arctan2(delta_y, delta_x) # yaw angle between (-PI, PI)[rad]
if (yaw < -PI/2):
yaw += 2*PI
list_yaw[min_index] = yaw
else:
list_yaw[min_index] = yawThe yaw value is significant because it is one of the inputs for the inverse kinematics function "lab_invk" that gives the set the joint angles based on the desire xyz coordinates.
The robot arm was commanded by the "move_block" function to pick and place the sticks and construct the bridge. The "move_block" function consists of several "move_arm" function that drives the robot arm to the desired position specified by input "dest" of the "move_arm" function.
"""
Move robot arm from one position to another
"""
def move_arm(pub_cmd, loop_rate, dest, vel, accel):
global thetas
global SPIN_RATE
error = 0
spin_count = 0
at_goal = 0
driver_msg = command()
driver_msg.destination = dest
driver_msg.v = vel
driver_msg.a = accel
driver_msg.io_0 = current_io_0
pub_cmd.publish(driver_msg)
loop_rate.sleep()
while(at_goal == 0):
if( abs(thetas[0]-driver_msg.destination[0]) < 0.0005 and \
abs(thetas[1]-driver_msg.destination[1]) < 0.0005 and \
abs(thetas[2]-driver_msg.destination[2]) < 0.0005 and \
abs(thetas[3]-driver_msg.destination[3]) < 0.0005 and \
abs(thetas[4]-driver_msg.destination[4]) < 0.0005 and \
abs(thetas[5]-driver_msg.destination[5]) < 0.0005 ):
at_goal = 1
#rospy.loginfo("Goal is reached!")
loop_rate.sleep()
if(spin_count > SPIN_RATE*5):
pub_cmd.publish(driver_msg)
rospy.loginfo("Just published again driver_msg")
spin_count = 0
spin_count = spin_count + 1
return errorThe "move_block" function is made up of several "move_arm" functions to pick and place the sticks build the bridge.
def move_block(pub_cmd, loop_rate, start_xw_yw_zw, target_xw_yw_zw, yaw, vel, accel):
"""
start_xw_yw_zw: where to pick up a block
target_xw_yw_zw: where to place the block
hint: you will use lab_invk(), gripper(), move_arm() functions to
pick and place a block
"""
# ========================= Student's code starts here =========================
# start xyz coordinates
start_above_xyz = [start_xw_yw_zw[0],start_xw_yw_zw[1], 100]
# global variable1
error = 0
vel = 4.0
accel = 4.0 # velocity and acceleration
# move to home
move_arm(pub_cmd, loop_rate, home, vel, accel)
# move to block (yaw goes to correct value)
move_arm(pub_cmd, loop_rate, lab_invk(start_xw_yw_zw[0],start_xw_yw_zw[1],start_xw_yw_zw[2], yaw*180/PI), vel, accel)
# turn on gripper
gripper(pub_cmd, loop_rate, suction_on)
#wait for 1 second
time.sleep(1.0)
# # check if block is sucked
# if (digital_in_0 == 0):
# gripper(pub_cmd, loop_rate, suction_off) # turn off gripper
# error = 1
# move_arm(pub_cmd, loop_rate, home, vel, accel)
# sys.exit() # move to home and terminate
# move straight up with block
move_arm(pub_cmd, loop_rate, lab_invk(start_above_xyz[0], start_above_xyz[1], start_above_xyz[2], yaw*180/PI), vel, accel)
# move to home with block
move_arm(pub_cmd, loop_rate, home, vel, accel)
# move block to destination
move_arm(pub_cmd, loop_rate, lab_invk(target_xw_yw_zw[0], target_xw_yw_zw[1], target_xw_yw_zw[2], 0), vel, accel)
# turn off gripper to drop the block
gripper(pub_cmd, loop_rate, suction_off)
#wait for 1 second
time.sleep(1.0)
# move back to home location
move_arm(pub_cmd, loop_rate, home_dest, vel, accel)
# move back to home location
move_arm(pub_cmd, loop_rate, home, vel, accel)
error = 0
# ========================= Student's code ends here ===========================
return errorThen finally, this "move_block" function was called 11 times in the "main" function to pick and place the 11 sticks to their respective destinations and construct the Leaning Bridge of Lire.
Video:
Comments