def unit_test(listener, br):
# Initialize
isExecute = True
rospy.sleep(0.5)
viz_pub = rospy.Publisher('/proposal_visualization_marker_array',
MarkerArray,
queue_size=10)
# data = posesrv('','')
#~ goToHome.prepGripperPicking()
#~define objId
TARGET = 'expo_eraser'
#~define obj poses
objInput = []
objInput.append(ik.helper.get_params_yaml('bin0_pose'))
objInput.append(ik.helper.get_params_yaml('bin1_pose'))
objInput.append(ik.helper.get_params_yaml('bin2_pose'))
#
#~unit test for flags 1 and 2 in bins 0 and 1 (with weight guard)
flag_list = [0, 1, 2]
bin_list = [0, 1, 2]
# objInput = [[1.0550236701965332, -0.40252241492271423, -0.018005974590778351, 0.0, 0.0, -1.0, 0.15000000596046448, 0.10999999940395355, -0.38268342614173889, -0.92387950420379639, 0.0, 0.22444444894790649]]
# binId = 0
# flag = 0
# bin_drop_id = binId
# (output_dict)=grasp(objInput=objInput[binId],
# listener=listener,
# br=br,
# isExecute=isExecute,
# objId=TARGET,
# binId=bin_drop_id,
# flag=flag,
# withPause = False)
# ~ bin_list = [0]
for binId in bin_list:
for flag in flag_list:
bin_list_drop = [binId]
(output_dict) = grasp(objInput=objInput[binId],
listener=listener,
br=br,
isExecute=isExecute,
objId=TARGET,
binId=binId,
flag=flag,
withPause=False,
viz_pub=viz_pub)
gripper.close()
def callback(data):
s = str(data)
print(s)
data = re.split('data: "| "| |"', s)
print(len(data))
print(data)
if len(data) == 3:
if data[1] == 'o':
gripper.open()
elif data[1] == 'c':
gripper.close()
else:
gripper.stop()
elif len(data) == 8:
for i in range(1, 7):
query[i] = float(data[i])
dequeue()
def try_suction(target_x, target_y, plans, qf, num_iter):
(continue_val, h1, h2, sidepos) = get_motion_param(target_x, target_y)
if continue_val == False:
return False, False
final_hand_gap = max_gripper_width
targetPositionList = [[target_x, sidepos, h1], [target_x, sidepos, h2]]
(continue_val, plans,
throwaway) = generate_plan(targetPositionList, plans, qf)
if continue_val == False:
return False, False
#set robot speed
setSpeedByName(speedName='faster')
hand_gap = 0
gripper.close()
execute_forward(plans, hand_gap)
#suction.start()
gripper.set_force(10)
gripper.grasp(move_pos=max_gripper_width)
gripper.close()
hand_gap = max_gripper_width
print '[SucDown] hand_gap:', hand_gap
execute_backward(plans, hand_gap, 0)
#time.sleep(4)
my_return = suction.check() or suction_override(objId)
if my_return == True:
#set robot speed
setSpeedByName(speedName='fast')
return (True, True)
else:
#suction.stop()
return (True, False)
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
#~Publish grasp proposal information
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_msgs)
comments_msgs = String()
comments_msgs.data = self.experiment_description
experiment_type_msgs = String()
experiment_type_msgs.data = self.experiment_type
self.experiment_comments_pub.publish(comments_msgs)
self.experiment_type_pub.publish(experiment_type_msgs)
else:
print('There are no grasp proposal')
self.execution_possible = False
return
#~Visualize grasp proposals
markers_msg = MarkerArray()
m0 = createDeleteAllMarker('pick_proposals')
markers_msg.markers.append(m0)
for i in range(10):
self.proposal_viz_array_pub.publish(markers_msg)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, self.all_grasp_proposals,
self.listener, self.br)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
self.listener, self.br, True)
#execute for grasp. Stop when the gripper is closed
self.back_img_list = self.capture_images()
#self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
num_it = 0
print('gripper open: ', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
while is_in_wrong_pose:
'''
self.retrieve_output = retrieve(listener=self.listener, br=self.br,
isExecute=self.isExecute, binId=container,
withPause=self.withPause, viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr, ws_object=self.weightSensor, isShake=False)
self.grasping_output = grasp(objInput=self.grasp_point, listener=self.listener, br=self.br,
isExecute=self.isExecute, binId=container, withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub, recorder=self.gdr, open_hand=False)
'''
# Motion heuristic
initial_dz = 0.05
dz = .022 #should be related to object length
ik.helper.move_cart(dz=initial_dz)
rospy.sleep(0.5)
ik.helper.move_cart(dz=-initial_dz)
rospy.sleep(0.5)
gripper.move(90)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
#should be done in the direction of the gripper plane
dx = 0.038 #.02*num_it
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
print('gripper_open', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
if is_in_wrong_pose:
gripper.move(90)
rospy.sleep(0.5)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
self.gdr.save_data_recorded = False
num_it += 1
is_in_wrong_pose = (
gripper.getGripperopening() < 0.04
) #and (gripper.getGripperopening() > 0.015)
self.retrieve_output = retrieve(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor,
isShake=False)
self.execution_possible = self.retrieve_output['execution_possible']
def closeGripper(forceThreshold):
# call gripper node, close
gripper.close()
return 1
def flush_grasp(objPose = [1.95,1.25,1.4,0,0,0,1],
binNum=4,
objId = 0,
bin_contents = None,
robotConfig = None,
forceThreshold = 1,
isExecute = True,
withPause = False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
#tcp_offset_variables
#set robot speed
setSpeedByName(speedName = 'fast')
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm=np.array(obj_pose_tfm_list)
obj_pose_orient=obj_pose_tfm[0:3,0:3]
vertical_index=np.argmax(np.fabs(obj_pose_orient[2,0:3]))
object_dim=get_obj_dim(objId)
vertical_dim=object_dim[vertical_index]
s1=np.fabs(obj_pose_orient[1,0])*object_dim[0]
s2=np.fabs(obj_pose_orient[1,1])*object_dim[1]
s3=np.fabs(obj_pose_orient[1,2])*object_dim[2]
s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index]
horizontal_dim=s1+s2+s3-s4
hand_gap=0
gripper.close()
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset=.035
l2 = 0.43 #change this to edit where you think the cup is
l2 =.44
tip_hand_transform = [-vert_offset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# for i in range(5):
# rospy.sleep(0.1)
# br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth,bin_height,bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
object_depth=objPosition[0]-binMouth[0]
finger_length=.23
finger_width=.06
up_offset=.05
down_offset=0.005
cup_to_spatula=.08
hand_width=.15
max_gripper_width=.110
hand_top_offset=hand_width/2+vert_offset+.03
hand_bot_offset=hand_width/2-vert_offset+.015
side_offset=.03
binFloorHeight=binMouth[2]-bin_height/2
binCeilHeight=binMouth[2]+bin_height/2
min_height=binFloorHeight+lipHeight+finger_width/2+down_offset
desired_height=objPosition[2]
max_height=binCeilHeight-lipHeight-finger_width/2-down_offset
target_height=desired_height
if target_height<min_height:
target_height=min_height
if target_height>max_height:
target_height=max_height
bin_sideways=binMouth[1]
sidepos=objPosition[1]
horz_offset=0.015
#this determines bin sides based on bin input
binSmall=binMouth[1]-bin_width/2
binLarge=binMouth[1]+bin_width/2
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
binRight=binSmall
binLeft=binLarge
final_hand_gap=110
if sidepos>bin_sideways:
#this stuff is what happens if the object is to the left
print 'Object is to the left'
if binNum==0 or binNum==3 or binNum==6 or binNum==9:
print 'Object is in that weird rail corner'
return False
#turn the suction cup so that it is sideways left
#scoopOrientation = [.5,-.5,.5,-.5]
scoopOrientation = [0.5,.5,.5,.5]
#side_waypoint1=binRight+cup_to_spatula+.01
#side_waypoint2=sidepos+horizontal_dim-side_offset
#side_waypoint2=binLeft-horizontal_dim-horz_offset
side_waypoint1=binLeft-hand_top_offset+horz_offset
if sidepos-horizontal_dim<binLeft-final_hand_gap:
print 'this is not a flush object'
return False
else:
#this stuff is what happens if the object is to the right
print 'Object is to the right'
if binNum==2 or binNum==5 or binNum==8 or binNum==11:
print 'Object is in that weird rail corner'
return False
#turn the suction cup so that it is sideways right
#scoopOrientation = [0.5,.5,.5,.5]
scoopOrientation = [.5,-.5,.5,-.5]
#side_waypoint1=binLeft-cup_to_spatula-.01
#side_waypoint2=sidepos-horizontal_dim+side_offset
#side_waypoint2=binRight+horizontal_dim+horz_offset
side_waypoint1=binRight+hand_top_offset-horz_offset
if sidepos+horizontal_dim>binRight+final_hand_gap:
print 'this is not a flush object'
return False
targetPositionList=[
[binMouth[0]-.15, side_waypoint1, min_height],
[binMouth[0]+.04, side_waypoint1, min_height],
[binMouth[0]+.3, side_waypoint1, min_height],
[binMouth[0]+.3, side_waypoint1, min_height+.03],
[binMouth[0]-.1, side_waypoint1, min_height+.03]]
qf=q0
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
frontOfObjectPtOutOfBin = targetPosition
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print 'move to COM in y successful'
print 'tcp at:'
print(targetPosition)
#plan.visualize(hand_param=hand_gap)
plans.append(plan)
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print 'move to COM in y fail'
return False
#print plan.q_traj
qf = plan.q_traj[-1]
print qf
for numOfPlan in range(0, 2):
if isExecute:
plans[numOfPlan].visualize(hand_param=final_hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
#time.sleep(2.5)
#suction.start()
final_hand_gap=110
gripper.set_force(20)
gripper.grasp(move_pos=final_hand_gap)
for numOfPlan in range(2, 3):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
gripper.set_force(50)
rospy.sleep(0.1)
gripper.move(move_pos=0)
hand_gap=final_hand_gap
print hand_gap
## retreat
#for numOfPlan in range(0, len(plans)-1):
# plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
# if isExecute:
# pauseFunc(withPause)
# plans[len(plans)-numOfPlan-1].executeBackward()
# suction.stop()
for numOfPlan in range(3, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
return True
def suction_side(objPose = [1.95,1.25,1.4,0,0,0,1],
binNum=4,
objId = 0,
bin_contents = None,
robotConfig = None,
shelfPosition = [1.9019,0.00030975,-0.503],
forceThreshold = 1,
isExecute = True,
withPause = False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
#rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
#tcp_offset_variables
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm=np.array(obj_pose_tfm_list)
obj_pose_orient=obj_pose_tfm[0:3,0:3]
vertical_index=np.argmax(np.fabs(obj_pose_orient[2,0:3]))
object_dim=get_obj_dim(objId)
vertical_dim=object_dim[vertical_index]
s1=np.fabs(obj_pose_orient[1,0])*object_dim[0]
s2=np.fabs(obj_pose_orient[1,1])*object_dim[1]
s3=np.fabs(obj_pose_orient[1,2])*object_dim[2]
s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index]
horizontal_dim=s1+s2+s3-s4
hand_gap=0
gripper.close()
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset=.035
l2 = 0.43 #change this to edit where you think the cup is
l2 =.44
tip_hand_transform = [-vert_offset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# for i in range(5):
# rospy.sleep(0.1)
# br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br, pose=tip_hand_transform, frame_id='target_pose', parent_frame_id='tip', npub=1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth,bin_height,bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
object_depth=objPosition[0]-binMouth[0]
finger_length=.23
finger_width=.08
up_offset=.05
down_offset=.01
cup_to_spatula=.08
hand_width=.15
max_gripper_width=.110
hand_top_offset=hand_width/2+vert_offset+.015
hand_bot_offset=hand_width/2-vert_offset+.015
up_down_adjust=.025
side_offset=.03
binFloorHeight=binMouth[2]-bin_height/2
binCeilHeight=binMouth[2]+bin_height/2
min_height=binFloorHeight+lipHeight+finger_width/2+down_offset
desired_height=objPosition[2]
max_height=binCeilHeight-lipHeight-finger_width/2-down_offset-up_down_adjust
target_height=desired_height
if target_height<min_height:
target_height=min_height
if target_height>max_height:
target_height=max_height
bin_sideways=binMouth[1]
sidepos=objPosition[1]
horz_offset=0.01
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall,binLarge)=getSides(binNum,listener)
binRight=binSmall
binLeft=binLarge
if sidepos>bin_sideways:
#this stuff is what happens if the object is to the left
print '[SucSide] Object is to the left'
#turn the suction cup so that it is sideways left
scoopOrientation = [.5,-.5,.5,-.5]
side_waypoint1a=binRight+cup_to_spatula+.01
side_waypoint1b=sidepos-horizontal_dim/2-2*horz_offset
if side_waypoint1a>side_waypoint1b:
side_waypoint1=side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1=side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos+horizontal_dim-side_offset
side_waypoint2=binLeft-horizontal_dim-horz_offset
if side_waypoint2<side_waypoint1:
if side_waypoint2<side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1=side_waypoint2
else:
#this stuff is what happens if the object is to the right
print '[SucSide] Object is to the right'
#turn the suction cup so that it is sideways right
scoopOrientation = [0.5,.5,.5,.5]
side_waypoint1a=binLeft-cup_to_spatula-.01
side_waypoint1b=sidepos+horizontal_dim/2+2*horz_offset
if side_waypoint1a<side_waypoint1b:
side_waypoint1=side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1=side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos-horizontal_dim+side_offset
side_waypoint2=binRight+horizontal_dim+horz_offset
if side_waypoint2>side_waypoint1:
if side_waypoint2>side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1=side_waypoint2
targetPositionList=[
[binMouth[0]-.15, side_waypoint1, target_height+up_down_adjust],
[objPosition[0], side_waypoint1, target_height+up_down_adjust],
[objPosition[0], side_waypoint2, target_height+up_down_adjust],
[objPosition[0], side_waypoint2, target_height]]
qf=q0
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
frontOfObjectPtOutOfBin = targetPosition
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[SucSide] move to COM in y successful'
print '[SucSide] tcp at:', targetPosition
plan.visualize(hand_param=hand_gap)
plans.append(plan)
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[SucSide] move to COM in y fail'
return (False, False)
#print plan.q_traj
qf = plan.q_traj[-1]
print '[SucSide] qf:', qf
#set robot speed
setSpeedByName(speedName = 'faster')
for numOfPlan in range(0, len(plans)):
if numOfPlan >=len(plans)-2:
setSpeedByName(speedName = 'fast')
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
suction.start()
final_hand_gap=110
gripper.set_force(12)
gripper.grasp(move_pos=final_hand_gap)
gripper.close()
hand_gap=final_hand_gap
print '[SucSide] hand_gap:', hand_gap
continue_suction=suction_items(objId)
if continue_suction:
print '[SucSide] object is of type that suction side will try to remove from bin'
else:
print '[SucSide] object is to big to remove from bin'
## retreat
for numOfPlan in range(0, len(plans)-1):
plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
if isExecute:
pauseFunc(withPause)
plans[len(plans)-numOfPlan-1].executeBackward()
if not continue_suction:
suction.stop()
rospy.sleep(3)
print '[SucSide] Is suction in contact still? Lets see:'
print '[SucSide] suction.check(): ', suction.check()
print '[SucSide] suction.check(): ', suction.check()
if suction.check():
#set robot speed
print '[SucSide] got item. continuing. suction'
setSpeedByName(speedName = 'fast')
return (True, True)
else:
print '[SucSide] did not get item. Stopping suction'
suction.stop()
return (True, False)
def try_suction(target_x,target_y,qf,num_iter):
object_depth=target_x-binMouth[0]
sidepos=min(max(target_y,small_limit),large_limit)
if object_depth<finger_length:
print 'shallow suction'
h1=binCeilHeight-lipHeight-cup_to_spatula
h2=binFloorHeight+vertical_dim-down_offset
else:
print 'deep suction'
h1=binCeilHeight-lipHeight-hand_top_offset
h2a=binFloorHeight+vertical_dim-down_offset
h2b=binFloorHeight+hand_bot_offset+lipHeight
h2=max(h2a,h2b)
h2=max(h2,binFloorHeight)
if h2>h1:
print 'cant go in'
return False, False
final_hand_gap=max_gripper_width
targetPositionList=[
[binMouth[0]-.15, sidepos, h1],
[target_x, sidepos, h1],
[target_x, sidepos, h2]]
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
planner = IK(q0 = qf, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation,
tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print 'Plan number:',tp_index+1
if s:
print 'Plan calculated successfully'
plans.append(plan)
else:
print 'Plan calulation failed'
return (False, False)
qf = plan.q_traj[-1]
#set robot speed
setSpeedByName(speedName = 'faster')
hand_gap=0
gripper.close()
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
suction.start()
gripper.set_force(10)
gripper.grasp(move_pos=max_gripper_width)
gripper.close()
hand_gap=max_gripper_width
print hand_gap
## retreat
if num_iter==0:
plan_offset=2
else:
plan_offset=0
for numOfPlan in range(0, len(plans)-plan_offset):
plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
if isExecute:
#backward_plan=plans[len(plans)-numOfPlan-1]
#q_first=backward_plan[0]
#q_last=backward_plan[len(backward_plan)-1]
#if(abs(q_first[5]-q_last[5])>math.pi/2):
# print 'something weird is going on with joint 5 rotation'
# break
pauseFunc(withPause)
plans[len(plans)-numOfPlan-1].executeBackward()
time.sleep(4)
my_return=suction.check() or suction_override(objId)
if my_return==True:
#set robot speed
setSpeedByName(speedName = 'fast')
return (True, True)
else:
suction.stop()
return (True, False)
#!/usr/bin/env python
import gripper
from ik.helper import Timer
gripper.homing()
gripper.open(speed=50)
with Timer('close50'):
gripper.close(speed=50)
with Timer('open50'):
gripper.open(speed=50)
with Timer('close100'):
gripper.close(speed=100)
with Timer('open100'):
gripper.open(speed=100)
with Timer('close200'):
gripper.close(speed=200)
with Timer('open200'):
gripper.open(speed=200)
with Timer('close400'):
gripper.close(speed=400)
with Timer('open400'):
gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
def suction_down(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
forceThreshold=1,
isExecute=True,
withPause=True):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm = np.array(obj_pose_tfm_list)
obj_pose_orient = obj_pose_tfm[0:3, 0:3]
vertical_index = np.argmax(np.fabs(obj_pose_orient[2, 0:3]))
object_dim = get_obj_dim(objId)
object_dim = adjust_obj_dim(objId, object_dim)
vertical_dim = object_dim[vertical_index]
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) >= 6:
q0 = q0[
0:
6] # take first 6, because in virtual environmet there will be additional 2 hand joint
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset = .035
l2 = .44
tip_hand_transform = [
-vert_offset, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0.7071, 0, 0.7071, 0]
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, bin_height, bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
listener)
binMouth = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
hand_gap = 0
gripper.close()
finger_length = .23
finger_width = .08
up_offset = .05
down_offset = -.025
cup_to_spatula = .08
hand_width = .15
max_gripper_width = 110
hand_top_offset = hand_width / 2 + vert_offset + .01
hand_bot_offset = hand_width / 2 - vert_offset
#this determines bin stuff based on bin input
binFloorHeight = binMouth[2] - bin_height / 2
binCeilHeight = binMouth[2] + bin_height / 2
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall, binLarge) = getSides(binNum, listener)
horz_offset = 0
small_limit = binSmall + finger_width / 2 + horz_offset
large_limit = binLarge - finger_width / 2 - horz_offset
plans = []
plan = Plan()
if objPosition[1] < 0:
link6_angle = (q0[5] - math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518 - 2 * math.pi
]
else:
link6_angle = (q0[5] + math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518
]
#start_config=possible_start_config
start_config = [q0[0], q0[1], q0[2], q0[3], q0[4], link6_angle]
planner = IKJoint(q0=q0, target_joint_pos=start_config)
plan = planner.plan()
#plan.q_traj=[q0,start_config]
plans.append(plan)
qf = plan.q_traj[-1]
def get_motion_param(target_x, target_y):
object_depth = target_x - binMouth[0]
sidepos = min(max(target_y, small_limit), large_limit)
if object_depth < finger_length:
print '[SucDown] shallow suction'
h1 = binCeilHeight - lipHeight - cup_to_spatula
h2 = binFloorHeight + vertical_dim - down_offset
else:
print '[SucDown] deep suction, quitting'
h1 = binCeilHeight - lipHeight - hand_top_offset
h2a = binFloorHeight + vertical_dim - down_offset
h2b = binFloorHeight + hand_bot_offset + lipHeight
h2 = max(h2a, h2b)
return False, h1, h2, sidepos
h2 = max(h2, binFloorHeight)
if h2 > h1:
print '[SucDown] cant go in'
return False, h1, h2, sidepos
return True, h1, h2, sidepos
def generate_plan(targetPositionList, plans, qf):
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
planner = IK(q0=qf,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[SucDown] Plan number:', tp_index + 1
if s:
print '[SucDown] Plan calculated successfully'
plans.append(plan)
else:
print '[SucDown] Plan calulation failed'
return (False, plans, qf)
qf = plan.q_traj[-1]
return (True, plans, qf)
def execute_forward(plans, hand_gap):
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
def execute_backward(plans, hand_gap, plan_offset):
for numOfPlan in range(0, len(plans) - plan_offset):
plans[len(plans) - numOfPlan -
1].visualizeBackward(hand_param=hand_gap)
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
def try_suction(target_x, target_y, plans, qf, num_iter):
(continue_val, h1, h2, sidepos) = get_motion_param(target_x, target_y)
if continue_val == False:
return False, False
final_hand_gap = max_gripper_width
targetPositionList = [[target_x, sidepos, h1], [target_x, sidepos, h2]]
(continue_val, plans,
throwaway) = generate_plan(targetPositionList, plans, qf)
if continue_val == False:
return False, False
#set robot speed
setSpeedByName(speedName='faster')
hand_gap = 0
gripper.close()
execute_forward(plans, hand_gap)
#suction.start()
gripper.set_force(10)
gripper.grasp(move_pos=max_gripper_width)
gripper.close()
hand_gap = max_gripper_width
print '[SucDown] hand_gap:', hand_gap
execute_backward(plans, hand_gap, 0)
#time.sleep(4)
my_return = suction.check() or suction_override(objId)
if my_return == True:
#set robot speed
setSpeedByName(speedName='fast')
return (True, True)
else:
#suction.stop()
return (True, False)
target_offset = get_test_offset(objId)
target_x_list = [
objPosition[0], objPosition[0] - target_offset, objPosition[0],
objPosition[0], objPosition[0] + target_offset
]
target_y_list = [
objPosition[1], objPosition[1], objPosition[1] + target_offset,
objPosition[1] - target_offset, objPosition[1]
]
count = 0
suction_succeed = False
overall_plan_succeed = False
(continue_val, h1, h2,
sidepos) = get_motion_param(target_x=target_x_list[0],
target_y=target_y_list[0])
if continue_val == False:
return False, False
#targetPositionList=[
#[binMouth[0]-.15, sidepos, h1],
#[binMouth[0]-.15, sidepos, h1],
#[target_x_list[0], sidepos, h1]]
targetPositionListA = [[binMouth[0] - .15, sidepos, h1],
[binMouth[0] - .15, sidepos, h1]]
targetPositionListB = [[target_x_list[0], sidepos, h1]]
(continue_val, plans1, qf) = generate_plan(targetPositionListA, plans, qf)
if continue_val == False:
return False, False
(continue_val, plans2, qf) = generate_plan(targetPositionListB, [], qf)
if continue_val == False:
return False, False
setSpeedByName(speedName='yolo')
execute_forward(plans1, 0)
setSpeedByName(speedName='faster')
execute_forward(plans2, 0)
suction.start()
#time.sleep(6)
#plans_store=plans
plans = []
for count in range(0, len(target_x_list)):
(plan_succeed,
suction_succeed) = try_suction(target_x=target_x_list[count],
target_y=target_y_list[count],
plans=plans,
qf=qf,
num_iter=count)
if suction_succeed:
overall_plan_succeed = True
break
#return (plan_succeed,suction_succeed)
if plan_succeed == True:
overall_plan_succeed = True
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
qf = APCrobotjoints.position
if len(qf) < 6:
continue
else:
break
print '[SucDown] qf:', hand_gap
plans = []
count = count + 1
execute_backward(plans2, 0, 0)
if not suction_succeed:
suction.stop()
return (overall_plan_succeed, suction_succeed)
def suction_side(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
forceThreshold=1,
isExecute=True,
withPause=False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
#rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
#tcp_offset_variables
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm = np.array(obj_pose_tfm_list)
obj_pose_orient = obj_pose_tfm[0:3, 0:3]
vertical_index = np.argmax(np.fabs(obj_pose_orient[2, 0:3]))
object_dim = get_obj_dim(objId)
vertical_dim = object_dim[vertical_index]
s1 = np.fabs(obj_pose_orient[1, 0]) * object_dim[0]
s2 = np.fabs(obj_pose_orient[1, 1]) * object_dim[1]
s3 = np.fabs(obj_pose_orient[1, 2]) * object_dim[2]
s4 = np.fabs(obj_pose_orient[1,
vertical_index]) * object_dim[vertical_index]
horizontal_dim = s1 + s2 + s3 - s4
hand_gap = 0
gripper.close()
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset = .035
l2 = 0.43 #change this to edit where you think the cup is
l2 = .44
tip_hand_transform = [
-vert_offset, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# for i in range(5):
# rospy.sleep(0.1)
# br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br,
pose=tip_hand_transform,
frame_id='target_pose',
parent_frame_id='tip',
npub=1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, bin_height, bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
listener)
binMouth = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
object_depth = objPosition[0] - binMouth[0]
finger_length = .23
finger_width = .08
up_offset = .05
down_offset = .01
cup_to_spatula = .08
hand_width = .15
max_gripper_width = .110
hand_top_offset = hand_width / 2 + vert_offset + .015
hand_bot_offset = hand_width / 2 - vert_offset + .015
up_down_adjust = .025
side_offset = .03
binFloorHeight = binMouth[2] - bin_height / 2
binCeilHeight = binMouth[2] + bin_height / 2
min_height = binFloorHeight + lipHeight + finger_width / 2 + down_offset
desired_height = objPosition[2]
max_height = binCeilHeight - lipHeight - finger_width / 2 - down_offset - up_down_adjust
target_height = desired_height
if target_height < min_height:
target_height = min_height
if target_height > max_height:
target_height = max_height
bin_sideways = binMouth[1]
sidepos = objPosition[1]
horz_offset = 0.01
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall, binLarge) = getSides(binNum, listener)
binRight = binSmall
binLeft = binLarge
if sidepos > bin_sideways:
#this stuff is what happens if the object is to the left
print '[SucSide] Object is to the left'
#turn the suction cup so that it is sideways left
scoopOrientation = [.5, -.5, .5, -.5]
side_waypoint1a = binRight + cup_to_spatula + .01
side_waypoint1b = sidepos - horizontal_dim / 2 - 2 * horz_offset
if side_waypoint1a > side_waypoint1b:
side_waypoint1 = side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1 = side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos+horizontal_dim-side_offset
side_waypoint2 = binLeft - horizontal_dim - horz_offset
if side_waypoint2 < side_waypoint1:
if side_waypoint2 < side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1 = side_waypoint2
else:
#this stuff is what happens if the object is to the right
print '[SucSide] Object is to the right'
#turn the suction cup so that it is sideways right
scoopOrientation = [0.5, .5, .5, .5]
side_waypoint1a = binLeft - cup_to_spatula - .01
side_waypoint1b = sidepos + horizontal_dim / 2 + 2 * horz_offset
if side_waypoint1a < side_waypoint1b:
side_waypoint1 = side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1 = side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos-horizontal_dim+side_offset
side_waypoint2 = binRight + horizontal_dim + horz_offset
if side_waypoint2 > side_waypoint1:
if side_waypoint2 > side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1 = side_waypoint2
targetPositionList = [
[binMouth[0] - .15, side_waypoint1, target_height + up_down_adjust],
[objPosition[0], side_waypoint1, target_height + up_down_adjust],
[objPosition[0], side_waypoint2, target_height + up_down_adjust],
[objPosition[0], side_waypoint2, target_height]
]
qf = q0
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
frontOfObjectPtOutOfBin = targetPosition
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[SucSide] move to COM in y successful'
print '[SucSide] tcp at:', targetPosition
plan.visualize(hand_param=hand_gap)
plans.append(plan)
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[SucSide] move to COM in y fail'
return (False, False)
#print plan.q_traj
qf = plan.q_traj[-1]
print '[SucSide] qf:', qf
#set robot speed
setSpeedByName(speedName='faster')
for numOfPlan in range(0, len(plans)):
if numOfPlan >= len(plans) - 2:
setSpeedByName(speedName='fast')
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
suction.start()
final_hand_gap = 110
gripper.set_force(12)
gripper.grasp(move_pos=final_hand_gap)
gripper.close()
hand_gap = final_hand_gap
print '[SucSide] hand_gap:', hand_gap
continue_suction = suction_items(objId)
if continue_suction:
print '[SucSide] object is of type that suction side will try to remove from bin'
else:
print '[SucSide] object is to big to remove from bin'
## retreat
for numOfPlan in range(0, len(plans) - 1):
plans[len(plans) - numOfPlan -
1].visualizeBackward(hand_param=hand_gap)
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
if not continue_suction:
suction.stop()
rospy.sleep(3)
print '[SucSide] Is suction in contact still? Lets see:'
print '[SucSide] suction.check(): ', suction.check()
print '[SucSide] suction.check(): ', suction.check()
if suction.check():
#set robot speed
print '[SucSide] got item. continuing. suction'
setSpeedByName(speedName='fast')
return (True, True)
else:
print '[SucSide] did not get item. Stopping suction'
suction.stop()
return (True, False)
#!/usr/bin/env python
import gripper
from ik.helper import Timer
gripper.homing()
gripper.open(speed=50)
with Timer('close50'):
gripper.close(speed=50)
with Timer('open50'):
gripper.open(speed=50)
with Timer('close100'):
gripper.close(speed=100)
with Timer('open100'):
gripper.open(speed=100)
with Timer('close200'):
gripper.close(speed=200)
with Timer('open200'):
gripper.open(speed=200)
with Timer('close400'):
gripper.close(speed=400)
with Timer('open400'):
gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
grasp_noise_msgs = Float32MultiArray()
grasp_noise_msgs.data = self.grasp_noise
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_msgs)
self.grasp_noise_pub.publish(grasp_noise_msgs)
comments_msgs = String()
comments_msgs.data = self.experiment_description
experiment_type_msgs = String()
experiment_type_msgs.data = self.experiment_type
self.experiment_comments_pub.publish(comments_msgs)
self.experiment_type_pub.publish(experiment_type_msgs)
if self.grasp_point is None:
print(
'It was suppose to do grasping, but there is no grasp proposal'
)
self.execution_possible = False
return
if self.visionType != 'real':
self.callFakeGrasping(prob=0.8, container=container)
return
#~visualize grasp proposals
markers_msg = MarkerArray()
m0 = createDeleteAllMarker('pick_proposals')
markers_msg.markers.append(m0)
for i in range(10):
self.proposal_viz_array_pub.publish(markers_msg)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, self.all_grasp_proposals,
self.listener, self.br)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
self.listener, self.br, True)
#execute for grasp. Stop when the gripper is closed
if self.is_control:
back_img_list = self.controller.capture_images()
#self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]
self.background_images = self.capture_images()
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
if self.is_record == True:
self.gdr.save_item(item_name='grasp_noise_std_dev',
data=self.grasp_std)
self.gdr.save_item(item_name='grasp_noise', data=self.grasp_noise)
is_in_wrong_pose = (gripper.getGripperopening() < 0.03) #raw_input()
if is_in_wrong_pose:
self.retrieve_output = retrieve(
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor)
gripper.open()
gripper.close()
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
if self.is_control:
if gripper.getGripperopening() > 0.017:
print('[Planner]: ', gripper.getGripperopening())
# WE PAUSE THE RECOORDER TO SAVE DATA
#self.gdr.pause_recording()
#find new and improved grasp points
best_grasp_dict, initial_score = self.controller.control_policy(
back_img_list,
smirror=self.smirror,
use_COM=self.use_COM,
use_raw=self.use_raw)
# self.controller.visualize_actions(with_CAM = False)
#self.controller.visualize_best_action(with_CAM = False)
#pdb.set_trace()
#save network information action_dict and best_action_dict
#WE UNPAUSE THE RECORDER
#self.gdr.replay_recording()
self.gdr.save_item(item_name='initial_score',
data=initial_score)
self.gdr.save_item(item_name='best_grasp_dict',
data=best_grasp_dict)
self.gdr.save_item(item_name='action_dict',
data=self.controller.action_dict)
self.gdr.save_item(item_name='best_action_dict',
data=self.controller.best_action_dict)
#go for new grasp PointgraspPose
self.grasping_output = grasp_correction(
self.grasp_point, best_grasp_dict['delta_pos'],
self.listener, self.br)
second_best_grasp_dict, final_score = self.controller.control_policy(
back_img_list,
smirror=self.smirror,
use_COM=self.use_COM,
use_raw=self.use_raw)
self.gdr.save_item(item_name='final_score', data=final_score)
self.gdr.save_item(item_name='second_best_grasp_dict',
data=second_best_grasp_dict)
self.gdr.save_data_recorded = True
else:
self.gdr.save_data_recorded = False
#frank hack for double grasping
if self.experiment_type == 'is_data_collection':
if gripper.getGripperopening() > 0.017:
# self.grasping_output = grasp_correction(self.grasp_point, np.array([0,0,0]), self.listener, self.br)
self.gdr.save_data_recorded = True
else:
self.gdr.save_data_recorded = False
self.retrieve_output = retrieve(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor)
self.execution_possible = self.retrieve_output['execution_possible']
def closeGripper(forceThreshold):
# call gripper node, close
gripper.close()
return 1
