def goToMouth(robotConfig = None,
binNum = 0,
isExecute = True,
withPause = False):
## robotConfig: current time robot configuration
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [0, 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]
tcpPosHome = tcpPos
# set home orientation
gripperOri = [0, 0.7071, 0, 0.7071]
# move to bin mouth
distFromShelf = 0.15
mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
q_initial = robotConfig
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToMouth] move to bin mouth successful'
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToMouth] move to bin mouth fail'
return None
qf = plan.q_traj[-1]
## open gripper fully
openGripper()
return plan
def scoop(objPose = [1.95,0.25,1.4], objId = 0, binNum=3, robotConfig=[0,0,0,0,0,0], shelfPosition = [1.91,0,-0.50], forceThreshold = 1, isExecute = False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix
## objId: object identifier
## robotConfig: current time robot configuration. List of joint angles, in radians
## shelfPosition: shelf position in world frame. This is a list.
## force threshold: amount of force needed to say we have a grasp
## initialize listener rospy
## Copy pasta hasta la pasta
rospy.init_node('listener', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
##hasta la copy la patio
## stop pasta
# shelf variables
pretensionDelta = 0.00 #how far we push nail down
lipHeight = 0.035 #height of the lip of the bin
# plan store
plans = [] #list of plans generated each plan is an object
## get object position (world frame)
objPosition = getObjCOM(objPose, objId) #
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6
l2 = 0.45 #how far tcp is away from link 6
tip_hand_transform = [0, 0, l2, 0,0,0] #something used for planning in drake, I don't get it
# to be updated when we have a hand design finalized
# broadcast frame attached to tcp
rospy.sleep(0.1) #.1 second delay so broadcasting works. syncing with ROS
#broadcasting transformation between tcp and link 6 on ROS
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)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
#tcp position as a list
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0, 0.7071, 0, 0.7071]
# set first target to move gripper in front of the object and adjust height to middle of bin
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, binFloorHeight) = getBinMouthAndFloor(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]
targetPosition = [binMouth[0], objPosition[1], binMouth[2]] #align the position of the tcp relative to the object before the scoop
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
#q0 is initial configuration
#target_tip_pos is where we want the tcp to be
#target_tip_ori is the tip orientation
#tip hand transform is see above
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success() #true if successful
if s:
print 'move to COM in y successful'
print 'tcp at:'
print(targetPosition)
plan.visualize() #display in arvix
plans.append(plan)
if isExecute:
plan.execute() #this executes the plan for reals
else:
print 'move to COM in y fail'
#after this point, it's just a sequence of points for the plan
qf = plan.q_traj[-1] #this is the final configuration of the object
# set second target, go over the lip of the bin
interiorLipBin = [0,0.37,0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
deltaX = np.add(-targetPosition[0],interiorLipBin.pose.position.x)
targetPosition = np.add(targetPosition, [deltaX,0,0])
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'move to inside the lip success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'move to inside the lip fail'
qf = plan.q_traj[-1]
## open gripper fully
openGripper()
## push spatula against base of bin (pre-tension)
binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
q_initial = qf
deltaH = targetPosition[2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight
targetPosition = np.add(targetPosition, [0,0,-deltaH+wristWidth])
scoopOrientation = [0, 0.7071+0.11/2, 0, 0.7071-0.11/2]
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'push finger against floor of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'push finger against floor of bin fail'
qf = plan.q_traj[-1]
## perform bin length stroke to middle
q_initial = qf
targetPosition = np.add(targetPosition, [0.20,0,-lipHeight])
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke middle of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'stroke middle of bin fail'
qf = plan.q_traj[-1]
plans.append(plan)
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [0.20,0,0])
scoopOrientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke middle to end of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'stroke middle to end of bin fail'
qf = plan.q_traj[-1]
plans.append(plan)
## close gripper
closeGripper(forceThreshold)
## retreat
#pdb.set_trace() #TRACEEEEE
#this does each plan backwards so that the hand can leave the bin
#VERY IMPORTANT
for numOfPlan in range(0, len(plans)):
plans[len(plans)-numOfPlan-1].visualizeBackward()
if isExecute:
for numOfPlan in range(0, len(plans)):
plans[len(plans)-numOfPlan-1].executeBackward()
def scoop(objPose=[1.95, 0.25, 1.4],
objId=0,
binNum=3,
robotConfig=[0, 0, 0, 0, 0, 0],
shelfPosition=[1.91, 0, -0.50],
forceThreshold=1,
isExecute=False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix
## objId: object identifier
## robotConfig: current time robot configuration. List of joint angles, in radians
## shelfPosition: shelf position in world frame. This is a list.
## force threshold: amount of force needed to say we have a grasp
## initialize listener rospy
## Copy pasta hasta la pasta
rospy.init_node('listener', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
##hasta la copy la patio
## stop pasta
# shelf variables
pretensionDelta = 0.00 #how far we push nail down
lipHeight = 0.035 #height of the lip of the bin
# plan store
plans = [] #list of plans generated each plan is an object
## get object position (world frame)
objPosition = getObjCOM(objPose, objId) #
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6
l2 = 0.45 #how far tcp is away from link 6
tip_hand_transform = [
0, 0, l2, 0, 0, 0
] #something used for planning in drake, I don't get it
# to be updated when we have a hand design finalized
# broadcast frame attached to tcp
rospy.sleep(0.1) #.1 second delay so broadcasting works. syncing with ROS
#broadcasting transformation between tcp and link 6 on ROS
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)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
#tcp position as a list
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0, 0.7071, 0, 0.7071]
# set first target to move gripper in front of the object and adjust height to middle of bin
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, binFloorHeight) = getBinMouthAndFloor(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
]
targetPosition = [
binMouth[0], objPosition[1], binMouth[2]
] #align the position of the tcp relative to the object before the scoop
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
#q0 is initial configuration
#target_tip_pos is where we want the tcp to be
#target_tip_ori is the tip orientation
#tip hand transform is see above
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success() #true if successful
if s:
print 'move to COM in y successful'
print 'tcp at:'
print(targetPosition)
plan.visualize() #display in arvix
plans.append(plan)
if isExecute:
plan.execute() #this executes the plan for reals
else:
print 'move to COM in y fail'
#after this point, it's just a sequence of points for the plan
qf = plan.q_traj[-1] #this is the final configuration of the object
# set second target, go over the lip of the bin
interiorLipBin = [0, 0.37,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map',
listener)
deltaX = np.add(-targetPosition[0], interiorLipBin.pose.position.x)
targetPosition = np.add(targetPosition, [deltaX, 0, 0])
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'move to inside the lip success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'move to inside the lip fail'
qf = plan.q_traj[-1]
## open gripper fully
openGripper()
## push spatula against base of bin (pre-tension)
binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf',
'map', listener)
q_initial = qf
deltaH = targetPosition[
2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight
targetPosition = np.add(targetPosition, [0, 0, -deltaH + wristWidth])
scoopOrientation = [0, 0.7071 + 0.11 / 2, 0, 0.7071 - 0.11 / 2]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'push finger against floor of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'push finger against floor of bin fail'
qf = plan.q_traj[-1]
## perform bin length stroke to middle
q_initial = qf
targetPosition = np.add(targetPosition, [0.20, 0, -lipHeight])
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke middle of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'stroke middle of bin fail'
qf = plan.q_traj[-1]
plans.append(plan)
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [0.20, 0, 0])
scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke middle to end of bin success'
print 'tcp at:'
print(targetPosition)
plans.append(plan)
plan.visualize()
if isExecute:
plan.execute()
else:
print 'stroke middle to end of bin fail'
qf = plan.q_traj[-1]
plans.append(plan)
## close gripper
closeGripper(forceThreshold)
## retreat
#pdb.set_trace() #TRACEEEEE
#this does each plan backwards so that the hand can leave the bin
#VERY IMPORTANT
for numOfPlan in range(0, len(plans)):
plans[len(plans) - numOfPlan - 1].visualizeBackward()
if isExecute:
for numOfPlan in range(0, len(plans)):
plans[len(plans) - numOfPlan - 1].executeBackward()
def scoop(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=3,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9116, -0.012498, -0.4971],
forceThreshold=1,
isExecute=True,
withPause=True,
withVisualize=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
setSpeedByName(speedName='faster')
joint_topic = '/joint_states'
planSuccess = True
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
if binNum < 3:
pretensionDelta = 0.03
if binNum > 2 and binNum < 6:
pretensionDelta = 0.009
if binNum > 5 and binNum < 9:
pretensionDelta = 0.009
if binNum > 8:
pretensionDelta = 0.03
#pretensionDelta = 0.00
lipHeight = 0.025
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
tcpXOffset = 0.018
l2 = 0.47
tip_hand_transform = [
tcpXOffset, 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
]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0, 0.7071, 0, 0.7071]
# set first target to move gripper in front of the object and adjust height to middle of bin
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, binFloorHeight) = getBinMouthAndFloor(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
]
verticalOffsetLip = 0.00 # we need this so we don't damage the sucker
wH = 0.075
targetPosition = [
binMouth[0], objPosition[1],
binMouth[2] + tcpXOffset - wH + lipHeight - pretensionDelta
]
## check to make sure we are inside the bin and not colliding with sides:
minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum)
leftWall = coordinateFrameTransform([leftWall, 0, 0], 'shelf', 'map',
listener)
leftWall = leftWall.pose.position.y
rightWall = coordinateFrameTransform([rightWall, 0, 0], 'shelf', 'map',
listener)
rightWall = rightWall.pose.position.y
interiorLipBin = [0, 0.40,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map',
listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.20
sStroke = 0.19
binLengthSafety = 0.015
if targetPosition[1] + 0.04 > leftWall:
interiorLipBin = [0, 0.36,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] - 0.04 < leftWall:
interiorLipBin = [0, 0.36,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] + binLengthSafety > leftWall:
targetPosition[1] = leftWall
if targetPosition[1] - binLengthSafety < rightWall:
targetPosition[1] = rightWall
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
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() # Plan 0
if s:
print '[Scoop] move to COM in y successful'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Scoop] move to COM in y fail'
return (False, False)
qf = plan.q_traj[-1]
## push spatula against base of bin (pre-tension)
binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf',
'map', listener)
q_initial = qf
percentTilt = 0.1 # 10 percent
scoopOrientation = [
0, 0.7071 * (1 + percentTilt), 0, 0.7071 * (1 - percentTilt)
]
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() # Plan 1
if s:
print '[Scoop] reorient the hand'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] reorient the hand fail'
return (False, False)
qf = plan.q_traj[-1]
# set second target, go over the lip of the bin
deltaX = np.add(-targetPosition[0], stepOverLip)
targetPosition = np.add(targetPosition, [deltaX, 0, 0])
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() # Plan 2
if s:
print '[Scoop] move to inside the lip success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] move to inside the lip fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to middle
q_initial = qf
targetPosition = np.add(targetPosition, [fStroke, 0, -lipHeight])
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() # Plan 3
if s:
print '[Scoop] stroke middle of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [sStroke, 0, 0])
scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
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() # Plan 4
if s:
print '[Scoop] stroke middle to end of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle to end of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## close gripper
#~ closeGripper(forceThreshold)
closeGripper(forceThreshold)
execution_possible = True
## execute
isNotInCollision = True
for numOfPlan in range(0, len(plans)):
plans[numOfPlan].visualize()
if isExecute:
if numOfPlan == 3:
openGripper()
pauseFunc(withPause)
isNotInCollision = plans[numOfPlan].execute()
if numOfPlan == 3:
closeGripper(forceThreshold)
plans[numOfPlan].executeBackward()
openGripper()
plans[numOfPlan].execute()
if not isNotInCollision:
planFailNum = numOfPlan
print '[Scoop] collision detected'
break
if numOfPlan == 4:
closeGripper(forceThreshold)
rospy.sleep(0.5)
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) == 2 or len(q0) == 8:
q0 = q0[-2:] # get last 2
break
gripper_q0 = np.fabs(q0)
drop_thick = 0.000001 # finger gap =0.002m = .5 mm
if gripper_q0[0] < drop_thick:
print '[Scoop] ***************'
print '[Scoop] Could not grasp'
print '[Scoop] ***************'
execution_possible = False
else:
print '[Scoop] ***************'
print '[Scoop] Grasp Successful'
print '[Scoop] ***************'
execution_possible = True
if not isNotInCollision:
for numOfPlan in range(0, len(planFailNum)):
plans[planFailNum - numOfPlan].executeBackward()
## retreat
for numOfPlan in range(0, len(plans)):
if withVisualize:
plans[len(plans) - numOfPlan - 1].visualizeBackward()
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
return True, execution_possible
def goToBin(binNum=0,
robotConfig=None,
objectiveBinPos=[1.2, 0, 0.6],
isExecute=True,
withPause=False,
withSuction=False,
counter=0):
## 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
planSuctionSuccess = False
planGraspSuccess = False
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [
0, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# 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='map',
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
]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
if not withSuction:
gripperOri = [0, 0.7071, 0, 0.7071]
if withSuction:
print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
gripperOri = [
pose_world.pose.orientation.x, pose_world.pose.orientation.y,
pose_world.pose.orientation.z, pose_world.pose.orientation.w
]
# move back 10 cms to avoid object with shelf collision
distAwayFromBin = 0.1
targetPosition = [tcpPos[0] - distAwayFromBin, tcpPos[1], tcpPos[2]]
q_initial = robotConfig
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
print '[goToBin] tcp at:', targetPosition
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] move away from bin fail'
return False
qf = plan.q_traj[-1]
if binNum > 8:
#plan = Plan()
#plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] # should use IKJoint
planner = IKJoint(
q0=qf,
target_joint_pos=[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745])
plan = planner.plan()
print '[goToBin] going home because risky to go straight to objective bin'
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
# move above objective bin, approx
use_JointPos = False
if use_JointPos:
planner = IKJoint(
q0=qf, target_joint_pos=[0, -0.2953, 0.4462, 0, 0.8292, 1.5707])
plan = planner.plan()
#plan = Plan()
#plan.q_traj = [[0, -0.2953, 0.4462, 0, 0.8292, 1.5707]] # should use IKJoint
s = True
else:
if not withSuction:
gripperOri = [0.7071, 0.7071, 0, 0]
locationAboveBinCOM = [
0, 0, 0.15
] # define over the lip distance in shelf frame
targetPosition = [
objectiveBinPos[0] + locationAboveBinCOM[0],
objectiveBinPos[1] + locationAboveBinCOM[1],
objectiveBinPos[2] + locationAboveBinCOM[2]
]
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move to above bin success'
planSuctionSuccess = True
plans.append(plan)
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
return False
if withSuction:
bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map',
listener)
targetPosition = [
bin_xyz.pose.position.x, bin_xyz.pose.position.y,
bin_xyz.pose.position.z
]
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[goToBin] setting joints for suction'
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
if q0[5] < 0:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834,
3.1415 - 2 * math.pi
]
else:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415
]
# plan_n = Plan()
# plan_n.q_traj=[possible_start_config]
planner = IKJoint(target_joint_pos=possible_start_config)
plan_n = planner.plan()
plan_n.visualize()
qf = plan_n.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan_n.execute()
## go down and release object
if not withSuction:
q_initial = qf
objectiveBinPos[1] = (counter - 1) * 0.036 - 0.2
targetPosition = [
objectiveBinPos[0], objectiveBinPos[1], objectiveBinPos[2]
]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] go to xyz of bin with vertical bias success'
plans.append(plan)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] go to xyz of bin with vertical bias fail'
qf = plan.q_traj[-1]
openGripper()
## open gripper fully
# rospy.sleep(0.5)
# openGripper()
rospy.sleep(0.5)
suction.stop()
return True
def goToBin(binNum = 0,
robotConfig = None,
objectiveBinPos = [1.2,0,0.6],
isExecute = True,
withPause = False,
withSuction = False,
counter = 0):
## 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
planSuctionSuccess = False
planGraspSuccess = False
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [0, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# 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='map', 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]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
if not withSuction:
gripperOri = [0, 0.7071, 0, 0.7071]
if withSuction:
print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
gripperOri = [pose_world.pose.orientation.x,pose_world.pose.orientation.y,pose_world.pose.orientation.z,pose_world.pose.orientation.w]
# move back 10 cms to avoid object with shelf collision
distAwayFromBin = 0.1
targetPosition = [tcpPos[0]-distAwayFromBin, tcpPos[1], tcpPos[2]]
q_initial = robotConfig
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
print '[goToBin] tcp at:', targetPosition
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] move away from bin fail'
return False
qf = plan.q_traj[-1]
if binNum > 8:
#plan = Plan()
#plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] # should use IKJoint
planner = IKJoint(q0 = qf, target_joint_pos=[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745])
plan = planner.plan()
print '[goToBin] going home because risky to go straight to objective bin'
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
# move above objective bin, approx
use_JointPos = False
if use_JointPos:
planner = IKJoint(q0 = qf, target_joint_pos=[0, -0.2953, 0.4462, 0, 0.8292, 1.5707])
plan = planner.plan()
#plan = Plan()
#plan.q_traj = [[0, -0.2953, 0.4462, 0, 0.8292, 1.5707]] # should use IKJoint
s = True
else:
if not withSuction:
gripperOri = [0.7071,0.7071,0,0]
locationAboveBinCOM = [0,0,0.15] # define over the lip distance in shelf frame
targetPosition = [objectiveBinPos[0]+locationAboveBinCOM[0],objectiveBinPos[1]+locationAboveBinCOM[1],objectiveBinPos[2]+locationAboveBinCOM[2]]
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move to above bin success'
planSuctionSuccess = True
plans.append(plan)
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
return False
if withSuction:
bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map', listener)
targetPosition = [bin_xyz.pose.position.x,bin_xyz.pose.position.y,bin_xyz.pose.position.z]
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[goToBin] setting joints for suction'
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
if q0[5]<0:
stackAngle = -(counter-1)*1.6*3.1415/180+9*3.1415/180
possible_start_config=[stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415-2*math.pi]
else:
stackAngle = -(counter-1)*1.6*3.1415/180+9*3.1415/180
possible_start_config=[stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415]
# plan_n = Plan()
# plan_n.q_traj=[possible_start_config]
planner = IKJoint(target_joint_pos=possible_start_config)
plan_n = planner.plan()
plan_n.visualize()
qf = plan_n.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan_n.execute()
## go down and release object
if not withSuction:
q_initial = qf
objectiveBinPos[1] = (counter-1)*0.036-0.2
targetPosition = [objectiveBinPos[0],objectiveBinPos[1],objectiveBinPos[2]]
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] go to xyz of bin with vertical bias success'
plans.append(plan)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] go to xyz of bin with vertical bias fail'
qf = plan.q_traj[-1]
openGripper()
## open gripper fully
# rospy.sleep(0.5)
# openGripper()
rospy.sleep(0.5)
suction.stop()
return True
def goToMouth(robotConfig=None, binNum=0, isExecute=True, withPause=False):
## robotConfig: current time robot configuration
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [
0, 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
]
tcpPosHome = tcpPos
# set home orientation
gripperOri = [0, 0.7071, 0, 0.7071]
# move to bin mouth
distFromShelf = 0.15
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
q_initial = robotConfig
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToMouth] move to bin mouth successful'
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToMouth] move to bin mouth fail'
return None
qf = plan.q_traj[-1]
## open gripper fully
openGripper()
return plan
def scoop(objPose = [1.95,0.25,1.4,0,0,0,1],
binNum=3,
objId = 0,
bin_contents = None,
robotConfig = None,
shelfPosition = [1.9116,-0.012498,-0.4971],
forceThreshold = 1,
isExecute = True,
withPause = True,
withVisualize = 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
setSpeedByName(speedName = 'faster')
joint_topic = '/joint_states'
planSuccess = True
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
if binNum <3:
pretensionDelta = 0.03
if binNum > 2 and binNum < 6:
pretensionDelta = 0.009
if binNum > 5 and binNum < 9:
pretensionDelta = 0.009
if binNum > 8:
pretensionDelta = 0.03
#pretensionDelta = 0.00
lipHeight = 0.025
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
tcpXOffset = 0.018
l2 = 0.47
tip_hand_transform = [tcpXOffset, 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]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0, 0.7071, 0, 0.7071]
# set first target to move gripper in front of the object and adjust height to middle of bin
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, binFloorHeight) = getBinMouthAndFloor(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]
verticalOffsetLip = 0.00 # we need this so we don't damage the sucker
wH = 0.075
targetPosition = [binMouth[0], objPosition[1], binMouth[2]+tcpXOffset-wH+lipHeight-pretensionDelta]
## check to make sure we are inside the bin and not colliding with sides:
minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum)
leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener)
leftWall = leftWall.pose.position.y
rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener)
rightWall = rightWall.pose.position.y
interiorLipBin = [0,0.40,0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.20
sStroke = 0.19
binLengthSafety = 0.015
if targetPosition[1] + 0.04 > leftWall:
interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] - 0.04 < leftWall:
interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] + binLengthSafety > leftWall:
targetPosition[1] = leftWall
if targetPosition[1] - binLengthSafety < rightWall:
targetPosition[1] = rightWall
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
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() # Plan 0
if s:
print '[Scoop] move to COM in y successful'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Scoop] move to COM in y fail'
return (False, False)
qf = plan.q_traj[-1]
## push spatula against base of bin (pre-tension)
binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
q_initial = qf
percentTilt = 0.1 # 10 percent
scoopOrientation = [0, 0.7071*(1+percentTilt), 0, 0.7071*(1-percentTilt)]
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() # Plan 1
if s:
print '[Scoop] reorient the hand'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] reorient the hand fail'
return (False, False)
qf = plan.q_traj[-1]
# set second target, go over the lip of the bin
deltaX = np.add(-targetPosition[0],stepOverLip)
targetPosition = np.add(targetPosition, [deltaX,0,0])
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() # Plan 2
if s:
print '[Scoop] move to inside the lip success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] move to inside the lip fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to middle
q_initial = qf
targetPosition = np.add(targetPosition, [fStroke,0,-lipHeight])
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() # Plan 3
if s:
print '[Scoop] stroke middle of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [sStroke,0,0])
scoopOrientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
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() # Plan 4
if s:
print '[Scoop] stroke middle to end of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle to end of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## close gripper
#~ closeGripper(forceThreshold)
closeGripper(forceThreshold)
execution_possible = True
## execute
isNotInCollision = True
for numOfPlan in range(0, len(plans)):
plans[numOfPlan].visualize()
if isExecute:
if numOfPlan == 3:
openGripper()
pauseFunc(withPause)
isNotInCollision = plans[numOfPlan].execute()
if numOfPlan == 3:
closeGripper(forceThreshold)
plans[numOfPlan].executeBackward()
openGripper()
plans[numOfPlan].execute()
if not isNotInCollision:
planFailNum = numOfPlan
print '[Scoop] collision detected'
break
if numOfPlan == 4:
closeGripper(forceThreshold)
rospy.sleep(0.5)
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) == 2 or len(q0) == 8:
q0 = q0[-2:] # get last 2
break
gripper_q0=np.fabs(q0)
drop_thick=0.000001 # finger gap =0.002m = .5 mm
if gripper_q0[0] < drop_thick:
print '[Scoop] ***************'
print '[Scoop] Could not grasp'
print '[Scoop] ***************'
execution_possible = False
else:
print '[Scoop] ***************'
print '[Scoop] Grasp Successful'
print '[Scoop] ***************'
execution_possible = True
if not isNotInCollision:
for numOfPlan in range(0, len(planFailNum)):
plans[planFailNum-numOfPlan].executeBackward()
## retreat
for numOfPlan in range(0, len(plans)):
if withVisualize:
plans[len(plans)-numOfPlan-1].visualizeBackward()
if isExecute:
pauseFunc(withPause)
plans[len(plans)-numOfPlan-1].executeBackward()
return True, execution_possible
