def main():
try:
pnp.goto_home(0.3, goal_tol=0.01, orientation_tol=0.1)
reset_gripper()
activate_gripper()
# # 255 = closed, 0 = open
# gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
# rospy.sleep(1.0)
group = pnp.group
current_pose = group.get_current_pose().pose
allow_replanning = True
planning_time = 10
status = pnp.go_to_pose_goal(pnp.q[0], pnp.q[1], pnp.q[2], pnp.q[3],
0.75, 0, current_pose.position.z,
allow_replanning, planning_time)
rospy.sleep(0.1)
print "\n", group.get_current_pose().pose.position
# gripper_to_pos(255, 60, 200, False) # GRIPPER TO POSITION 50
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def execute(self, userdata):
global pnp, done_onions, current_state
# rospy.loginfo('Executing state: Claim')
if userdata.counter >= 50:
userdata.counter = 0
return 'timed_out'
if len(userdata.color) == 0:
return 'not_found'
max_index = len(userdata.color)
print '\nMax index is = ', max_index
pnp.onion_index = 0
for i in range(max_index):
if len(userdata.y) >= i:
try:
if userdata.y[i] > -0.35 and userdata.y[i] < 0.25:
pnp.target_location_x = userdata.x[i]
pnp.target_location_y = userdata.y[i]
pnp.target_location_z = userdata.z[i]
pnp.onion_color = userdata.color[i]
pnp.onion_index = i
self.is_updated = True
break
else:
done_onions += 1
print '\nDone onions = ', done_onions
except IndexError:
pass
else:
print '\nSort complete!'
return 'completed'
if max_index == done_onions:
print '\nAll onions are sorted!'
return 'completed'
else:
done_onions = 0
if self.is_updated == False:
userdata.counter += 1
return 'not_updated'
else:
print '\n(x,y,z) after claim: ',pnp.target_location_x,pnp.target_location_y,pnp.target_location_z
reset_gripper()
activate_gripper()
userdata.counter = 0
current_state = vals2sid(ol=0, eefl=3, pred=2, listst=2)
rospy.sleep(0.05)
return 'updated'
def main():
try:
limb = 'right'
hover_distance = 0.15 # meters
# Starting Joint angles for right arm
starting_joint_angles = {
'right_j0': -0.041662954890248294,
'right_j1': -1.0258291091425074,
'right_j2': 0.0293680414401436,
'right_j3': 2.17518162913313,
'right_j4': -0.06703022873354225,
'right_j5': 0.3968371433926965,
'right_j6': 1.7659649178699421
}
pnp = PickAndPlace(limb)
# An orientation for gripper fingers to be overhead and parallel to the obj
overhead_orientation = Quaternion(x=0.7071057,
y=-0.7071029,
z=0.0023561,
w=-0.0012299)
# Move to the desired starting angles
print("Running. Ctrl-c to quit")
pnp.move_to_start(starting_joint_angles)
rospy.sleep(1)
reset_gripper()
activate_gripper()
# 255 = closed, 0 = open
gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 1 (HOVER POS)
0.665,
0.0,
0.15)
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 2 (HOVER POS)
0.665,
0.0,
0.002)
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 3 (PLUNGE AND PICK)
0.665,
0.0,
-0.001)
sleep(1.0)
gripper_to_pos(50, 60, 200, False) # GRIPPER TO POSITION 50
os.system('rosrun gazebo_ros_link_attacher attach.py'
) # ATTACH CUBE AND SAWYER EEF
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 4 (TRAVEL TO PLACE DESTINATION)
0.665,
0.04,
0.15)
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 5 (TRAVEL TO PLACE DESTINATION)
0.665,
0.5,
0.02)
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 6 (PLACE)
0.665,
0.5,
-0.055)
os.system('rosrun gazebo_ros_link_attacher detach.py'
) # DETACH CUBE AND SAWYER EEF
gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
sleep(1.0)
pnp.go_to_pose_goal(
0.7071029,
0.7071057,
0.0012299,
0.0023561, # GO TO WAYPOINT 7 (RETURN TO HOVER POS)
0.665,
0.5,
0.12)
rospy.sleep(1.0)
delete_gazebo_models()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def main():
global target_location_x,target_location_y,pnp,location_EE,\
ConveyorWidthRANGE_LOWER_LIMIT, onion_attached, onion_index,\
ObjectPoseZ_RANGE_UPPER_LIMIT, location_claimed_onion,\
SAWYERRANGE_UPPER_LIMIT, gap_needed_to_pick
ConveyorWidthRANGE_LOWER_LIMIT = rospy.get_param(
"/ConveyorWidthRANGE_LOWER_LIMIT")
ObjectPoseZ_RANGE_UPPER_LIMIT = rospy.get_param(
"/ObjectPoseZ_RANGE_UPPER_LIMIT")
SAWYERRANGE_UPPER_LIMIT = rospy.get_param("/SAWYERRANGE_UPPER_LIMIT")
gap_needed_to_pick = rospy.get_param("/gap_needed_to_pick")
reset_gripper()
activate_gripper()
gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
rospy.Subscriber("cylinder_blocks_poses", cylinder_blocks_poses,
callback_poses)
rospy.Subscriber("current_cylinder_blocks", Int8MultiArray,
callback_modelname)
# attach and detach service
attach_srv.wait_for_service()
detach_srv.wait_for_service()
req.link_name_1 = "base_link"
req.model_name_2 = "sawyer"
req.link_name_2 = "right_l6"
# pub_location_EE = rospy.Publisher('location_EE', Int64, queue_size=10)
# pub_location_Onion = rospy.Publisher('location_claimed_onion', Int64, queue_size=10)
srv_moverobot = rospy.Service('move_sawyer', move_robot,
handle_move_sawyer)
srv_updatestate = rospy.Service('update_state', update_state,
handle_update_state)
print "Ready to move sawyer."
# rospy.spin()
last_location_EE = -1
last_location_claimed_onion = -1
str_locEE = "Unknown"
str_locO = "Unknown"
# deciding orientation for roll action
# global pnp
# goto_home(0.3)
# joint_goal = pnp.group.get_current_joint_values()
# joint_goal[5]=joint_goal[5]-1.2
# joint_goal[6]=joint_goal[6]-1.57
# pnp.go_to_joint_goal(joint_goal,False,0.5,\
# goal_tol=0.02,orientation_tol=0.02)
# sleep(5.0)
# current_pose = pnp.group.get_current_pose()
# q_local=[0,0,0,0]
# q_local[0] = current_pose.pose.orientation.x
# q_local[1] = current_pose.pose.orientation.y
# q_local[2] = current_pose.pose.orientation.z
# q_local[3] = current_pose.pose.orientation.w
# print "q_local:"
# print q_local
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
global location_EE
# print "onion_index,location_EE:"\
# +str((onion_index,location_EE))
loc = location_EE
if loc == 0:
str_locEE = "Conv"
else:
if loc == 1:
str_locEE = "InFront"
else:
if loc == 2:
str_locEE = "AtBin"
else:
if loc == 3:
str_locEE = "Home"
else:
str_locEE = "Unknown"
if location_EE != last_location_EE:
print "str_locEE:" + str_locEE
# pub_location_EE.publish(location_EE)
last_location_EE = location_EE
rate.sleep()
#### For refering to what param values worked first time #####
# print "goto_home()"
# goto_home(0.4,goal_tol=0.1,\
# orientation_tol=0.1)
## PICKING ##
# hovertime=1.0
# print "hover()"
# success=hover(hovertime)
# print "lowergripper()"
# reset_gripper()
# activate_gripper()
# gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
# lowergripper()
# sleep(5.0)
# attach_srv.call(req)
# sleep(2.0)
# print "liftgripper()"
# liftgripper()
# sleep(2.0)
## INPECTION ##
# print "lookNrotate()"
# lookNrotate()
## PLACING ##
# print "goto_bin()"
# goto_bin()
# sleep(5.0)
# detach_srv.call(req)
## ROLLING ##
# print "hover() on leftmost onion"
# hover()
# sleep(2.0)
# print "roll()"
# roll()
# sleep(50.0)
return
def execute(self, userdata):
global pnp, total_onions, done_onions
while len(userdata.x) == 0:
rospy.sleep(0.1)
# rospy.loginfo('Executing state: Claim')
if userdata.counter >= 50:
userdata.counter = 0
return 'timed_out'
if len(userdata.color) == 0:
return 'not_found'
max_index = len(userdata.color)
# print("I'm here 1")
# # attach and detach service
# attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
# attach_srv.wait_for_service()
# detach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
# detach_srv.wait_for_service()
# pnp.req.model_name_1 = None
# pnp.req.link_name_1 = "base_link"
# pnp.req.model_name_2 = "sawyer"
# pnp.req.link_name_2 = "right_l6"
pnp.target_location_x = userdata.x[pnp.onion_index]
pnp.target_location_y = userdata.y[pnp.onion_index]
pnp.target_location_z = userdata.z[pnp.onion_index]
# pnp.bad_onions = [i for i in range(pnp.onion_index, max_index) if (oi.color[i] == 0)]
# print("Bad onion indices are: ", pnp.bad_onions)
for i in range(total_onions):
if len(done_onions) == total_onions:
return 'completed'
if i in done_onions:
pass
else:
onion_guess = "onion_" + str(i)
# 255 = closed, 0 = open
gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
rospy.sleep(0.1)
# print("I'm here 2")
self.is_updated = True
# rospy.wait_for_service('/gazebo/get_model_state')
# try:
# model_coordinates = rospy.ServiceProxy(
# '/gazebo/get_model_state', GetModelState)
# # print 'Onion name guessed: ', onion_guess
# onion_coordinates = model_coordinates(onion_guess, "").posepnp.req.model_name_1
# except rospy.ServiceException, e:
# print "Service call failed: %s" % e
# if pnp.target_location_y - 0.05 <= onion_coordinates.position.y <= pnp.target_location_y + 0.05:
# if pnp.target_location_x - 0.05 <= onion_coordinates.position.x <= pnp.target_location_x + 0.05:
# pnp.req.model_name_1 = onion_guess
# done_onions.append(i)
# self.is_updated = True
# break
if self.is_updated == False:
userdata.counter += 1
return 'not_updated'
else:
# print "Onion name set as: ", onion_guess
reset_gripper()
activate_gripper()
userdata.counter = 0
rospy.sleep(0.05)
return 'updated'
def execute(self, userdata):
global pnp, done_onions, current_state
# rospy.loginfo('Executing state: Claim')
if userdata.counter >= 50:
userdata.counter = 0
return 'timed_out'
if len(userdata.color) == 0:
return 'not_found'
max_index = len(userdata.color)
print '\nMax index is = ', max_index
pnp.onion_index = 0
for i in range(max_index):
if len(userdata.y) >= i: # The number of onions found is >= the index value I'm considering sorting now.
try:
if userdata.y[i] > -0.35 and userdata.y[i] < 0.25:
pnp.target_location_x = userdata.x[i]
pnp.target_location_y = userdata.y[i]
pnp.target_location_z = userdata.z[i]
pnp.onion_color = userdata.color[i]
pnp.onion_index = i
self.is_updated = True
break
else:
done_onions += 1
print '\nDone onions = ', done_onions
except IndexError:
pass
else:
print '\nCompleted this batch, moving on!'
if max_index == done_onions:
# print '\nAll onions are sorted!'
msg = Empty()
rospy.sleep(0.1)
self.conv_pub.publish(msg)
rospy.sleep(3.25) # With the conveyor speed controller knob at 40, takes around 3 secs to move to next batch.
self.conv_pub.publish(msg)
rospy.sleep(0.5)
userdata.x = []
userdata.y = []
userdata.z = []
userdata.color = []
userdata.counter = 0
max_index = 0
done_onions = 0
return 'move'
else:
done_onions = 0
if self.is_updated == False:
userdata.counter += 1
return 'not_updated'
else:
print '\n(x,y,z) after claim: ',pnp.target_location_x,pnp.target_location_y,pnp.target_location_z
reset_gripper()
activate_gripper()
userdata.counter = 0
current_state = vals2sid(ol=0, eefl=3, pred=2, listst=2)
rospy.sleep(0.05)
return 'updated'