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 push(objPose = [1.55,0.25,1.1,0,0,0,0],
binNum=0,
objId = 0,
bin_contents = None,
robotConfig=None,
shelfPosition = [1.9116,-0.012498,-0.4971],
forceThreshold = 1,
binDepthReach = 0.40,
isExecute = True,
withPause = False):
## objPose: world position and orientation frame attached to com of object
## 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
## initialize listener rospy
rospy.init_node('listener', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
## 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
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 topple orientation (rotate wrist)
toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] #[0, 0.7071, 0, 0.7071]
# if gripper open close it
closeGripper(forceThreshold)
# 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)
binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
binFloorHeight = binFloorHeight.pose.position.z
binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
# set offset past COM
# offset is 1/4 of object's height + half finger width
# note: (COM height - bin floor height) is half of the object's height
#pdb.set_trace() #TRACEEEEE
fingerWidth = 0.06
offsetFinger = fingerWidth/2
targetHeight = (objPosition[2] - binFloorHeight)*2
effect = 'slide'
if effect == 'topple':
offsetEffect = targetHeight*3/4
elif effect == 'slide':
offsetEffect = targetHeight*1/4
else:
offsetEffect = targetHeight*1/4 #Default is slide, made three cases for flexibility
offset = offsetFinger + offsetEffect
targetPosition = [binMouth[0], objPosition[1], binFloorHeight+offset]
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s and effect == 'topple':
print 'move to COM in y and above COM in z successful'
elif s and effect == 'slide':
print 'move to COM in y and below COM in z successful'
else:
print 'move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility
if s:
print 'tcp at:'
print(targetPosition)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print 'move to COM in y and above COM in z fail'
qf = plan.q_traj[-1]
# set second target, go over the lip of the bin
#interiorLipBin = [0,0.39,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 = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
#plan = planner.plan()
#s = plan.success()
#if s:
# print 'move to inside the lip success'
# print 'tcp at:'
# print(targetPosition)
# plan.visualize()
# if isExecute:
# pauseFunc(withPause)
# plan.execute()
#else:
# print 'move to inside the lip fail'
#qf = plan.q_traj[-1]
## close gripper fully (Do we wish to topple with gripper slightly open?)
closeGripper(forceThreshold)
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [binDepthReach,0,0])
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke to end of bin success'
print 'tcp at:'
print(targetPosition)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print 'stroke to end of bin fail'
qf = plan.q_traj[-1]
## close gripper
closeGripper(forceThreshold)
## retreat
# go back along stroke
q_initial = qf
targetPosition = frontOfObjectPtOutOfBin
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print 'stroke to bin start success'
print 'tcp at:'
print(targetPosition)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print 'stroke to bin start fail'
qf = plan.q_traj[-1]
# go back to initial tcp position
q_initial = qf
targetPosition = frontOfObjectPtOutOfBin
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print 'return to mouth success'
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print 'return to mouth fail'
return False
def topple(objPose = [1.55,0.25,1.1,0,0,0,0],
binNum=0,
objId = 0,
bin_contents = None,
robotConfig = None,
shelfPosition = [1.9116,-0.012498,-0.4971],
forceThreshold = 1,
binDepthReach = 0.40,
isExecute = True,
withPause = True,
withVisualize = True):
## objPose: world position and orientation frame attached to com of object
## 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
planSuccess = True
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
setSpeedByName(speedName = 'faster')
joint_topic = '/joint_states'
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
temp_bias = 0.00 # TO-DO remove this:
# 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
l1 = 0.018
l2 = 0.470
tip_hand_transform = [l1, 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
toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
closeGripper(forceThreshold) # if gripper open close it
# 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)
binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
binFloorHeight = binFloorHeight.pose.position.z
binMouth = [pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
# set offset past COM
# offset is 1/4 of object's height + half finger width
# note: (COM height - bin floor height) is half of the object's height
fingerWidth = 0.06
offsetFinger = fingerWidth/2
targetHeight = (objPosition[2] - binFloorHeight)*2 # to be changed to be from an object
offsetEffect = targetHeight*3/4
offset = offsetEffect
vertToppleHeight = binFloorHeight+offset-temp_bias
## bounding box constraint
minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) # shelf frame
# trying to topple an object that is too tall?
maxHeight = coordinateFrameTransform([0,0,maxHeight], 'shelf', 'map', listener)
#pdb.set_trace()
targetPosition = [binMouth[0], objPosition[1], vertToppleHeight]
binDepthReach = 0.3
binHeightSaftey = 0.01
if vertToppleHeight + binHeightSaftey > maxHeight.pose.position.z:
fingerLength = 0.26
binDepth = 0.42
binDepthReach = binDepth - fingerLength + 0.08
toppleHeightWhenTall = binMouth[2] + 0.03
targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenTall]
print '[Topple] Object too tall, topple will not go all the way in'
# trying to topple an object that is too short?
minHeight = coordinateFrameTransform([0,0,minHeight], 'shelf', 'map', listener)
closeGripper(forceThreshold)
if vertToppleHeight - binHeightSaftey < minHeight.pose.position.z:
print '[Topple] Object too short, resetting topple height'
toppleHeightWhenShort = minHeight.pose.position.z + binHeightSaftey
targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenShort]
gripper.move(80,100)
# too far left or right?
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
binLengthSafety = 0.02
horizontalSaftey = 0.01
if targetPosition[1] + binLengthSafety > leftWall:
targetPosition[1] = leftWall - horizontalSaftey
if targetPosition[1] - binLengthSafety < rightWall:
targetPosition[1] = rightWall + horizontalSaftey
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
effect = 'topple'
if s and effect == 'topple':
print '[Topple] move to COM in y and above COM in z successful'
elif s and effect == 'slide':
print '[Topple] move to COM in y and below COM in z successful'
else:
print '[Topple] move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility
if s:
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Topple] move to COM in y and above COM in z fail'
planSuccess = False
qf = plan.q_traj[-1]
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [binDepthReach,0,0])
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[Topple] stroke to end of bin success'
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Topple] stroke to end of bin fail'
planSuccess = False
qf = plan.q_traj[-1]
## close gripper
closeGripper(forceThreshold)
## retreat
# go back along stroke
q_initial = qf
targetPosition = frontOfObjectPtOutOfBin
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[Topple] stroke to bin start success'
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Topple] stroke to bin start fail'
planSuccess = False
qf = plan.q_traj[-1]
# go back to initial tcp position
q_initial = qf
targetPosition = frontOfObjectPtOutOfBin
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[Topple] return to mouth success'
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Topple] return to mouth fail'
planSuccess = False
if not planSuccess:
return False, False
## execute
#~ for numOfPlan in range(0, len(plans)):
#~ plans[numOfPlan].visualize()
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plans[numOfPlan].execute()
## execute
isNotInCollision = True
for numOfPlan in range(0, len(plans)):
plans[numOfPlan].visualize()
if isExecute:
pauseFunc(withPause)
isNotInCollision = plans[numOfPlan].execute()
if not isNotInCollision:
print '[Topple] collision detected'
planFailNum = numOfPlan
break
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, False
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