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 manual_cal_1(binNum=0, strPos=None):
if strPos == None:
print 'No desired position entered'
return
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [
l1, 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)
# 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
orientation = [0.0, 0.7071, 0.0,
0.7071] # set topple orientation (rotate wrist)
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)
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
]
print 'TCP is at:', tcpPos
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
distFromShelf, binNum)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
listener)
mouthBin = [
mouthBin.pose.position.x, mouthBin.pose.position.y,
mouthBin.pose.position.z
]
# define modes here:
# go home
if strPos == 'home':
print 'planning to go home'
plan = Plan()
plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]]
plan.visualize()
raw_input('execute?')
plan.execute()
print 'done going home'
# go mouth
if strPos == 'mouth':
print 'planning to go to mouth'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go mid back bin
if strPos == 'shelf':
print 'planning to go to mid back of the bin'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mid back of the bin'
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 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 manual_cal_1(binNum = 0, strPos = None):
if strPos == None:
print 'No desired position entered'
return
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [l1, 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)
# 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
orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
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)
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]
print 'TCP is at:', tcpPos
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, binNum)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
# define modes here:
# go home
if strPos == 'home':
print 'planning to go home'
plan = Plan()
plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]]
plan.visualize()
raw_input('execute?')
plan.execute()
print 'done going home'
# go mouth
if strPos == 'mouth':
print 'planning to go to mouth'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go mid back bin
if strPos == 'shelf':
print 'planning to go to mid back of the bin'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mid back of the bin'
def main(argv=None):
rospy.init_node('calib', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json'
robot_touch_pts = readFromJson(filename)
binNums = [0,2,3,5,6,8,9,11]
nbin = len(binNums)
diff_sum = 0
# shelf_pose/x: 1.92421
# shelf_pose/y: -0.0124050312627
# shelf_pose/z: -0.513553368384
###########################################
print 'floor of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'bin',i,'(expected - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum/nbin
offset_x = diff_sum/nbin
print 'Please move the shelf in z by', offset_x
print '\twhich is', -0.513553368384 - offset_x
###########################################
diff_sum_left = 0
print 'left wall of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = leftWall - touch_pt[0] # examine difference in x
print 'left wall: bin',i,'(expected - robot_touch)=', diff
diff_sum_left += diff
print 'avg(diff) left wall=', diff_sum_left/nbin
###########################################
diff_sum_right = 0
print 'right wall of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = rightWall - touch_pt[0] # examine difference in x
print 'right wall: bin',i,'(expected - robot_touch)=', diff
diff_sum_right += diff
print 'avg(diff) right wall=', diff_sum_right/nbin
offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2
print 'Please move the shelf in y by', offset_y
print '\twhich is', 0.0124050312627 - offset_y
###########################################
diff_sum = 0
print 'lip to floor of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'row',i,'(bin floor - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum/nbin
offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2
print 'Please move the shelf in y by', offset_y
print '\twhich is', 0.0124050312627 - offset_y
########################################## front
diff_sum_front = 0
print 'front of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = rightWall - touch_pt[1] # examine difference in y
print 'right front wall: bin',i,'(expected - robot_touch)=', diff
diff_sum_front += diff
print 'avg(diff) right wall=', diff_sum_front/nbin
offset_x = diff_sum_front/nbin
print 'Please move the shelf in x by', offset_x
print '\twhich is', 1.92421 - offset_x #- 0.87/2
def push_back(objPose=[1.95,0.25,1.4,0,0,0,1],
binNum=4,
objId = 'crayola_64_ct',
bin_contents = ['paper_mate_12_count_mirado_black_warrior','crayola_64_ct','expo_dry_erase_board_eraser'],
robotConfig = None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute = True,
withPause = True):
#~ *****************************************************************
obj_dim=get_obj_dim(objId)
obj_dim=np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width=0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
(shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion)
shelf_pose_tfm=np.array(shelf_pose_tfm_list)
shelf_pose_orient=shelf_pose_tfm[0:3,0:3]
#~ print '[PushBack] shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos=shelf_pose_tfm[0:3,3]
#~ print '[PushBack] shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0])
shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1])
shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
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]
obj_pose_pos=obj_pose_tfm[0:3,3]
#~ print '[PushBack] obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0])
#~ print '[PushBack] obj_X'
#~ print obj_X
obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1])
#~ print '[PushBack] obj_Y'
#~ print obj_Y
obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2])
#~ print '[PushBack] obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z))
obj_pose_orient_norm=obj_pose_orient_norm.transpose()
#~ print '[PushBack] obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm)
#~ print '[PushBack] proj'
#~ print proj_vecY
max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir]>0:
hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir]
else:
hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir]
#~ print '[PushBack] hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec));
angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180/np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm=obj_dim[hand_norm_dir]
# print '[PushBack] dim along hand norm'
# print obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX=np.dot(shelf_X,obj_pose_orient_norm)
max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir]>0:
fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir]
else:
fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir]
#~ print '[PushBack] fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis=obj_dim[fing_axis_dir]
# print '[PushBack] obj_dim_along_fingAxis'
# print obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm)
max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ))
Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir]
#~ print '[PushBack] Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis=obj_dim[Zaxis_dir]
hand_Y=np.cross(hand_norm_vec,fing_axis_vec)
diag_dist=la.norm(np.array([obj_dim_along_hand_norm,obj_dim_along_fingAxis]))
# print'req obj diag dist=', diag_dist
#~ *****************************************************************
# Find the maximum distance to which we will push
other_diag=np.zeros(len(bin_contents))
for num_bin_contents in range(0, len(bin_contents)):
if bin_contents[num_bin_contents] != objId:
temp_obj_dim=get_obj_dim(objId)
other_diag[num_bin_contents]=la.norm(np.array([temp_obj_dim[0],temp_obj_dim[1]]))
max_diag,max_obj_ID=np.max(np.fabs(other_diag)), np.argmax(np.fabs(other_diag))
# print '[PushBack] max_obj_ID',max_diag
# print '[PushBack] max_obj_ID', bin_contents[max_obj_ID]
#*******************************************************************
bin_inner_cnstr=get_bin_inner_cnstr()
#*******************************************************************
#Check if we can do left push
diag_factor=0.75
obj_left_edge=obj_pose_pos[1]+diag_factor*diag_dist/2.0
binLeftWall=bin_inner_cnstr[binNum][1]
binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
binRightWall=bin_inner_cnstr[binNum][0]
binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
left_gap=binLeftWall_world.pose.position.y-obj_left_edge
# print '[PushBack] left_gap=', left_gap
fing_clearance=0.010 # It's like 5 mm on both sides of finger
bin_width=binLeftWall_world.pose.position.y-binRightWall_world.pose.position.y
tolerance_in_push=0.020
if left_gap+diag_dist/2.0-tolerance_in_push>bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False,False)
if left_gap > fing_width+fing_clearance:
left_push=True
print '[PushBack] left push is possible'
else:
left_push=False
print '[PushBack] left push is NOT possible'
# check if we can do right push
obj_right_edge=obj_pose_pos[1]-diag_factor*diag_dist/2.0
right_gap=obj_right_edge-binRightWall_world.pose.position.y
# print '[PushBack] right_gap', right_gap
fing_clearance=0.010 # It's like 5 mm on both sides of finger
if right_gap+diag_dist/2.0-tolerance_in_push>bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False,False)
if right_gap > fing_width+fing_clearance:
right_push=True
print '[PushBack] right push is possible'
else:
right_push=False
print '[PushBack] right push is NOT possible'
#*******************************************************************
#Find Push Points for left push
bin_face_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum)
bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map', listener)
tcp_Z_off=0.005 #lower down from center of the bin
push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z
push_side_off=1.0
if left_push:
left_pushPt_X=bin_face_pos_world.pose.position.x+0.05
left_pushPt_Y=binLeftWall_world.pose.position.y-push_side_off*left_gap
# left_Y_lim=binLeftWall_world.pose.position.y-
left_pushPt_Z=deepcopy(push_tcp_Z)
left_push_Pt=np.array([left_pushPt_X,left_pushPt_Y,left_pushPt_Z])
print '[PushBack] expected left push clearance=',binLeftWall_world.pose.position.y-left_pushPt_Y-fing_width/2.0
#Find Push Points for right push
if right_push:
right_pushPt_X=bin_face_pos_world.pose.position.x+0.05
right_pushPt_Y=binRightWall_world.pose.position.y+push_side_off*right_gap
right_pushPt_Z=deepcopy(push_tcp_Z)
right_push_Pt=np.array([right_pushPt_X,right_pushPt_Y,right_pushPt_Z])
print '[PushBack] expected right push clearance=',right_pushPt_Y-binRightWall_world.pose.position.y-fing_width/2.0
#Execute push trajectories
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient=spatula_down_orient
push_hand_opening=0.100
joint_topic = '/joint_states'
#~ *************************************************************
#Execure left push trajectory
if left_push:
# plan store
left_plans = []
# set tcp (same as that set in generate dictionary)
l1=0.0
l2=0.0
l3 = 0.47
tip_hand_transform = [l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
gripperOri=[0, 0.7071, 0, 0.7071]
pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (left push)'
left_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (left push)'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1=0.0
push_l2=0.0
push_l3 = 0.47
push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
distFromShelf = -0.015
InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [InbinPt.pose.position.x, left_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt
pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)
planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
left_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening*1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
left_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X=bin_face_pos_world.pose.position.x+0.43
push_stop_worldX=back_bin_X-max_diag-0.010
left_pushStop_Pt_pos=deepcopy(left_push_Pt)
left_pushStop_Pt_pos[0]=push_stop_worldX
#Push until the end
planner = IK(q0 = q_initial, target_tip_pos = left_pushStop_Pt_pos, target_tip_ori = push_orient,
joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
left_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualize()
pauseFunc(withPause)
left_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
left_plans[len(left_plans)-numOfPlan-1].executeBackward()
#*******************************************************************
if right_push:
# plan store
right_plans = []
# set tcp (same as that set in generate dictionary)
l1=0.0
l2=0.0
l3 = 0.47
tip_hand_transform = [l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
gripperOri=[0, 0.7071, 0, 0.7071]
pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (right push)'
right_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (right push)'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1=0.0
push_l2=0.0
push_l3 = 0.47
push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
distFromShelf = -0.015
InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [InbinPt.pose.position.x, right_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt
pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)
planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
right_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening*1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
right_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X=bin_face_pos_world.pose.position.x+0.42
push_stop_worldX=back_bin_X-max_diag-0.010
right_pushStop_Pt_pos=deepcopy(right_push_Pt)
right_pushStop_Pt_pos[0]=push_stop_worldX
#Push until the end
planner = IK(q0 = q_initial, target_tip_pos = right_pushStop_Pt_pos, target_tip_ori = push_orient,
joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
right_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False,False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualize()
pauseFunc(withPause)
right_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
right_plans[len(right_plans)-numOfPlan-1].executeBackward()
if right_push or left_push:
push_possible=True
else:
push_possible=False
return(push_possible, False)
def main(argv=None):
rospy.init_node('calib', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib.json'
robot_touch_floor_pts = readFromJson(filename)
num_rows = 4
diff_sum = 0
print 'floor of the shelves'
for i in range(num_rows):
pose = coordinateFrameTransform(robot_touch_floor_pts[i], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'row',i,'(expected - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum/num_rows
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_left_side.json'
robot_touch_left_pts = readFromJson(filename)
num_rows = 4
diff_sum_left = 0
print 'left wall of the shelves'
for i in range(num_rows):
pose = coordinateFrameTransform(robot_touch_left_pts[i], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i*3) # a bin at row i
diff = leftWall - touch_pt[0] # examine difference in x
print 'left wall: row',i,'(expected - robot_touch)=', diff
diff_sum_left += diff
print 'avg(diff) left wall=', diff_sum_left/num_rows
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right_side.json'
robot_touch_right_pts = readFromJson(filename)
num_rows = 4
diff_sum_right = 0
print 'right wall of the shelves'
for i in range(num_rows):
pose = coordinateFrameTransform(robot_touch_right_pts[i], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i*3) # a bin at row i
diff = rightWall - touch_pt[0] # examine difference in x
print 'right wall: row',i,'(expected - robot_touch)=', diff
diff_sum_right += diff
print 'avg(diff) right wall=', diff_sum_right/num_rows
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right.json'
robot_touch_lip_pts = readFromJson(filename)
num_rows = 4
diff_sum = 0
print 'lip to floor of the shelves'
for i in range(num_rows):
pose = coordinateFrameTransform(robot_touch_lip_pts[i], '/map', '/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'row',i,'(bin floor - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum/num_rows
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 main(argv=None):
rospy.init_node('calib', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
filename = os.environ[
'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json'
robot_touch_pts = readFromJson(filename)
binNums = [0, 2, 3, 5, 6, 8, 9, 11]
nbin = len(binNums)
diff_sum = 0
# shelf_pose/x: 1.92421
# shelf_pose/y: -0.0124050312627
# shelf_pose/z: -0.513553368384
###########################################
print 'floor of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map',
'/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition,
binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'bin', i, '(expected - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum / nbin
offset_x = diff_sum / nbin
print 'Please move the shelf in z by', offset_x
print '\twhich is', -0.513553368384 - offset_x
###########################################
diff_sum_left = 0
print 'left wall of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map',
'/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = leftWall - touch_pt[0] # examine difference in x
print 'left wall: bin', i, '(expected - robot_touch)=', diff
diff_sum_left += diff
print 'avg(diff) left wall=', diff_sum_left / nbin
###########################################
diff_sum_right = 0
print 'right wall of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map',
'/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = rightWall - touch_pt[0] # examine difference in x
print 'right wall: bin', i, '(expected - robot_touch)=', diff
diff_sum_right += diff
print 'avg(diff) right wall=', diff_sum_right / nbin
offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2
print 'Please move the shelf in y by', offset_y
print '\twhich is', 0.0124050312627 - offset_y
###########################################
diff_sum = 0
print 'lip to floor of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map',
'/shelf', listener)
touch_pt = pose2list(pose.pose)
(mouthposition,
binFloorHeight) = getBinMouthAndFloor(0, i) # a bin at row i
diff = binFloorHeight - touch_pt[2] # examine difference in z
print 'row', i, '(bin floor - robot_touch)=', diff
diff_sum += diff
print 'avg(diff)=', diff_sum / nbin
offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2
print 'Please move the shelf in y by', offset_y
print '\twhich is', 0.0124050312627 - offset_y
########################################## front
diff_sum_front = 0
print 'front of the shelves'
for i in binNums:
pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map',
'/shelf', listener)
touch_pt = pose2list(pose.pose)
(leftWall, rightWall) = getBinSideWalls(i) # a bin at row i
diff = rightWall - touch_pt[1] # examine difference in y
print 'right front wall: bin', i, '(expected - robot_touch)=', diff
diff_sum_front += diff
print 'avg(diff) right wall=', diff_sum_front / nbin
offset_x = diff_sum_front / nbin
print 'Please move the shelf in x by', offset_x
print '\twhich is', 1.92421 - offset_x #- 0.87/2
def manual_cal_2(binNum = 0, endBinNum = 11):
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
moveDistVertical = 0.08 #we could do 1 more cm
#set Speed
ik.ik.setSpeedByName('faster')
# 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(), 'calib', "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)
tcpPos =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
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]
print 'TCP is at:', tcpPos
print 'planning to go home'
# home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
# planner = IKJoint(target_joint_pos=home_joint_pose)
# plan = planner.plan()
# plan.visualize()
raw_input('execute?')
# plan.execute()
goToHome.goToHome()
print 'done going home'
calibBins = []
for x in range(binNum, endBinNum+1):
calibPts = []
if x == 1 or x == 4 or x == 7 or x == 10:
calibBins.append([])
continue
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, x)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
epsilon = 0.016
distFromShelf = distFromShelf - epsilon
# go to mouth of bin
print 'planning to go to mouth but a little out'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go to the mouth of the bin but actually inside
print 'planning to go to start position'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to start position'
OrientationList = []
targetPtList = []
captionList = []
ntouch = 5
# go down
captionList.append('go down')
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]-moveDistVertical]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go left
captionList.append('go left')
Orientation = [0.5, 0.5, 0.5, 0.5]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]+moveDistHorizontal, mouthBin[2]]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go right
captionList.append('go right')
Orientation = [0.5, -0.5, 0.5, -0.5]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]-moveDistHorizontal, mouthBin[2]]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go up
captionList.append('go up')
Orientation = [0.7071, 0, 0.7071, 0]
targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]+moveDistVertical]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go to mid back of the bin
captionList.append('go mid back')
Orientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
targetPt = [mouthBin[0]+0.30, mouthBin[1], mouthBin[2]-0.08]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
for touchInd in range(ntouch):
print captionList[touchInd]
targetPt = targetPtList[touchInd]
Orientation = OrientationList[touchInd]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going down, TELEOP TIME:'
raw_input('when done with teleop hit enter: ')
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
t = listener.getLatestCommonTime('calib', 'map')
(calibPoseTrans, calibPoseRot) = listener.lookupTransform('map','calib',t)
calibPose = list(calibPoseTrans) + list(calibPoseRot)
#~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
#~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
calibPts.append(calibPose)
raw_input('done recording position, hit enter to go back')
if s:
plan.executeBackward()
else:
print 'Please teleop back'
raw_input('done?')
calibBins.append(calibPts)
filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
with open(filename, 'w') as outfile:
json.dump(calibBins, outfile)
print 'planning to go home'
home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('faster')
plan.execute()
print 'done going home'
print 'dumped to file ', filename
def push_rotate(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId='crayola_64_ct',
bin_contents=None,
robotConfig=None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute=True,
withPause=True):
#~ *****************************************************************
obj_dim = get_obj_dim(objId)
obj_dim = np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width = 0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.2)
(shelf_position,
shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
shelf_pose_tfm = np.array(shelf_pose_tfm_list)
shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
#~ print 'shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos = shelf_pose_tfm[0:3, 3]
#~ print 'shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
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]
obj_pose_pos = obj_pose_tfm[0:3, 3]
#~ print 'obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
#~ print 'obj_X'
#~ print obj_X
obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
#~ print 'obj_Y'
#~ print obj_Y
obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
#~ print 'obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
obj_pose_orient_norm = obj_pose_orient_norm.transpose()
#~ print 'obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
#~ print 'proj'
#~ print proj_vecY
max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir] > 0:
hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
else:
hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]
#~ print 'hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
la.norm(hand_norm_vec))
angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180.0 / np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)
max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir] > 0:
fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
else:
fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]
#~ print 'fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)
max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
np.fabs(proj_vecZ))
Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]
#~ print 'Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis = obj_dim[Zaxis_dir]
hand_Y = np.cross(hand_norm_vec, fing_axis_vec)
#~ *****************************************************************
#################### DECISION STRATEGIES #########################
bin_inner_cnstr = get_bin_inner_cnstr()
proj_handNorm_ShelfX = np.dot(hand_norm_vec, shelf_X)
right_push = False
left_push = False
# print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm
# print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis
#
# print 'proj_handNorm_ShelfX'
# print proj_handNorm_ShelfX
if obj_dim_along_hand_norm > obj_dim_along_fingAxis:
#~ print 'proj_vecY'
#~ print proj_vecY
#~ print proj_vecY[hand_norm_dir]
#~
#~ print 'proj_vecX'
#~ print proj_vecX
#~ print proj_vecX[fing_axis_dir]
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
#~ else:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
if proj_handNorm_ShelfX > 0:
#Do left push
Ypush_mult = 1
left_push = True
print '[PushRotate] Left Push'
else:
#Do right push
Ypush_mult = -1
print '[PushRotate] right Push'
right_push = True
push_offset = (
obj_dim_along_hand_norm / 2.0
) #(Instead pf pushing on edge we will push 5 mm inside)
else:
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
#~ else:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
if proj_handNorm_ShelfX > 0:
#Do right push
Ypush_mult = -1
print '[PushRotate] right Push'
right_push = True
else:
#Do left push
Ypush_mult = 1
print '[PushRotate] Left Push'
left_push = True
push_offset = (obj_dim_along_fingAxis / 2.0)
num_intm_points = 5
push_x_intm = np.zeros(num_intm_points + 1)
push_y_intm = np.zeros(num_intm_points + 1)
backWall_shelf = bin_inner_cnstr[binNum][1]
backWall_world = coordinateFrameTransform([0, backWall_shelf, 0], 'shelf',
'map', listener)
back_check = backWall_world.pose.position.x - obj_dim_along_hand_norm / 2.0
binRightWall = bin_inner_cnstr[binNum][0]
binLeftWall = bin_inner_cnstr[binNum][1]
binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
'shelf', 'map', listener)
binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
'map', listener)
if left_push:
sideWall_check = binRightWall_world.pose.position.y + (
fing_width / 2.0) + (obj_dim_along_fingAxis)
if right_push:
sideWall_check = binLeftWall_world.pose.position.y - (
fing_width / 2.0) - (obj_dim_along_fingAxis)
for i in range(num_intm_points + 1):
push_x_intm[i] = obj_pose_pos[0] + (push_offset) * np.sin(
((90 / num_intm_points) * i * np.pi) / 180.0)
if push_x_intm[i] > back_check:
push_x_intm[i] = back_check
push_y_intm[i] = obj_pose_pos[1] + (Ypush_mult * push_offset) * np.cos(
((90 / num_intm_points) * i * np.pi) / 180.0)
if left_push:
if push_y_intm[i] < sideWall_check:
push_y_intm[i] = sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
if right_push:
if push_y_intm[i] > sideWall_check:
push_y_intm[i] = sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
push_x_intm = np.array(push_x_intm)
#~ print'push_x_intm'
#~ print push_x_intm
push_y_intm = np.array(push_y_intm)
#*******************************************************************
#Move the hand to the center of the bin and open the hand based on the height of the object
bin_mid_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
binFloorHeight_world = coordinateFrameTransform([0, 0, binFloorHeight],
'shelf', 'map', listener)
bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map',
listener)
#*******************************************************************
tcp_Z_off = 0.005
push_tcp_Z_shelfFrame = (
(bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
2.0) - tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform(
[0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z
push_z_intm = (push_tcp_Z) * np.ones(push_x_intm.size)
push_series_pos = np.vstack((push_x_intm, push_y_intm, push_z_intm))
push_series_pos = push_series_pos.transpose()
#~ print 'push_series_pos'
#~ print push_series_pos
#~ print push_series_pos[0,:]
#~ print push_series_pos[-1,:]
push_possible = True
#******************************************************************
fing_push_pt = 0.2 * obj_dim_along_ZAxis + binFloorHeight_world.pose.position.z
blade_tip_TCP_off = 0.038 # dist between finger inner end and spatual edge
push_hand_opening = 2 * (push_tcp_Z - blade_tip_TCP_off - fing_push_pt)
if push_hand_opening > grasp_range_lim:
push_hand_opening = grasp_range_lim
# Spatula finger should not hit the lip of the bin while pushing
lip_Z_shelf = bin_inner_cnstr[binNum][4]
lip_Z_world = coordinateFrameTransform([0, 0, lip_Z_shelf], 'shelf', 'map',
listener)
#~ lip_off=.025
#~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off
max_allowed_hand_opening_out = 2 * (push_tcp_Z -
lip_Z_world.pose.position.z)
max_allowed_hand_opening = max_allowed_hand_opening_out - 0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface
if push_hand_opening > max_allowed_hand_opening:
print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin'
push_hand_opening = max_allowed_hand_opening
#~ print 'push_hand_opening'
#~ print push_hand_opening
#~ #hand should not hit the side walls
#~ binRightWall,binLeftWall=find_shelf_walls(binNum)
#~
#~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
#~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
side_wall_clearance = 0.0
print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y
print '[PushRotate] right_hand_edge=', (push_y_intm[0] - fing_width -
side_wall_clearance)
print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y
print '[PushRotate] left_hand_edge=', (push_y_intm[0] + fing_width +
side_wall_clearance)
if push_y_intm[
0] - fing_width - side_wall_clearance < binRightWall_world.pose.position.y:
print '[PushRotate] Hand will hit the right wall, Push rotate not possible'
push_possible = False
if push_y_intm[
0] + fing_width + side_wall_clearance > binLeftWall_world.pose.position.y:
print '[PushRotate] Hand will hit the left wall, push rotate not possible'
push_possible = False
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient = deepcopy(spatula_down_orient)
#~ *************************************************************
if push_possible == True:
#Now we know where we wan to move TCP
#Let's do robot motion planning now
joint_topic = '/joint_states'
# plan store
plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushRotate] move to bin mouth in push-rotate code successful'
#plan.visualize()
plans.append(plan) # Plan 0
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[PushRotate] move to bin mouth in push-rotate code fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ moveGripper(0.1,100)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0 #0.035
push_l2 = -Ypush_mult * (
fing_width / 2.0
) # This should depend on the righ or left push conditions
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
#~ q_initial = robotConfig
# move just inside the bin with push orientation and open the hand suitable to push
distFromShelf = -0.01
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, push_series_pos[0, 1],
InbinPt.pose.position.z
] #matching world_Y of first push pt
print '[PushRotate] prepush_targetPosition=', prepush_targetPosition
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] move inside bin in push orient successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] move inside bin in push orient fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH ###############
# Open the gripper to dim calculated for pushing
print '[PushRotate] hand opening to'
print push_hand_opening * 1000.0
#~ moveGripper(push_hand_opening,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
#~ push_rotate_traj = Plan()
#~ push_rotate_traj.q_traj = qf
for i in range(0, push_x_intm.size):
pushpt_pos = push_series_pos[i, :].transpose()
pubFrame(br,
pose=pushpt_pos.tolist() + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
#~ pdb.set_trace()
# planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient,
# joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb)
planner = IK(q0=q_initial,
target_tip_pos=pushpt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] point inside push traj successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] point inside push traj fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
#~ if isExecute:
#~ pauseFunc(withPause)
#~ push_rotate_traj.execute()
#################### CLOSE THE GRIPPER GRASP #################
#~ moveGripper(0.0,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize()
pauseFunc(withPause)
plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[len(plans) - numOfPlan - 1].visualizeBackward()
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
#*******************************************************************
print '[PushRotate] push_successful'
return (push_possible, False)
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 manual_cal_2(binNum=0, endBinNum=11):
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [
l1, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
moveDistVertical = 0.08 #we could do 1 more cm
#set Speed
ik.ik.setSpeedByName('faster')
# 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(), 'calib', "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)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0,
0.7071] # set topple orientation (rotate wrist)
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
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
]
print 'TCP is at:', tcpPos
print 'planning to go home'
# home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
# planner = IKJoint(target_joint_pos=home_joint_pose)
# plan = planner.plan()
# plan.visualize()
raw_input('execute?')
# plan.execute()
goToHome.goToHome()
print 'done going home'
calibBins = []
for x in range(binNum, endBinNum + 1):
calibPts = []
if x == 1 or x == 4 or x == 7 or x == 10:
calibBins.append([])
continue
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
distFromShelf, x)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
listener)
mouthBin = [
mouthBin.pose.position.x, mouthBin.pose.position.y,
mouthBin.pose.position.z
]
epsilon = 0.016
distFromShelf = distFromShelf - epsilon
# go to mouth of bin
print 'planning to go to mouth but a little out'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go to the mouth of the bin but actually inside
print 'planning to go to start position'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2]]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to start position'
OrientationList = []
targetPtList = []
captionList = []
ntouch = 5
# go down
captionList.append('go down')
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1],
mouthBin[2] - moveDistVertical
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go left
captionList.append('go left')
Orientation = [0.5, 0.5, 0.5, 0.5]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1] + moveDistHorizontal,
mouthBin[2]
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go right
captionList.append('go right')
Orientation = [0.5, -0.5, 0.5, -0.5]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1] - moveDistHorizontal,
mouthBin[2]
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go up
captionList.append('go up')
Orientation = [0.7071, 0, 0.7071, 0]
targetPt = [
mouthBin[0] + distFromShelf, mouthBin[1],
mouthBin[2] + moveDistVertical
]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
# go to mid back of the bin
captionList.append('go mid back')
Orientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
targetPt = [mouthBin[0] + 0.30, mouthBin[1], mouthBin[2] - 0.08]
OrientationList.append(Orientation)
targetPtList.append(targetPt)
for touchInd in range(ntouch):
print captionList[touchInd]
targetPt = targetPtList[touchInd]
Orientation = OrientationList[touchInd]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('fast')
plan.execute()
if not s:
print 'could not find plan'
print 'done going down, TELEOP TIME:'
raw_input('when done with teleop hit enter: ')
#~ rospy.sleep(0.1)
#~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
#~ rospy.sleep(0.1)
t = listener.getLatestCommonTime('calib', 'map')
(calibPoseTrans,
calibPoseRot) = listener.lookupTransform('map', 'calib', t)
calibPose = list(calibPoseTrans) + list(calibPoseRot)
#~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
#~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
calibPts.append(calibPose)
raw_input('done recording position, hit enter to go back')
if s:
plan.executeBackward()
else:
print 'Please teleop back'
raw_input('done?')
calibBins.append(calibPts)
filename = os.environ[
'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
with open(filename, 'w') as outfile:
json.dump(calibBins, outfile)
print 'planning to go home'
home_joint_pose = [
-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715,
0.1061231983, -0.1045031463
]
planner = IKJoint(target_joint_pos=home_joint_pose)
plan = planner.plan()
plan.visualize()
raw_input('execute?')
ik.ik.setSpeedByName('faster')
plan.execute()
print 'done going home'
print 'dumped to file ', filename
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 push_rotate(objPose=[1.95,0.25,1.4,0,0,0,1],
binNum=4,
objId = 'crayola_64_ct',
bin_contents = None,
robotConfig = None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute = True,
withPause = True):
#~ *****************************************************************
obj_dim=get_obj_dim(objId)
obj_dim=np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width=0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.2)
(shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion)
shelf_pose_tfm=np.array(shelf_pose_tfm_list)
shelf_pose_orient=shelf_pose_tfm[0:3,0:3]
#~ print 'shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos=shelf_pose_tfm[0:3,3]
#~ print 'shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0])
shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1])
shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
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]
obj_pose_pos=obj_pose_tfm[0:3,3]
#~ print 'obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0])
#~ print 'obj_X'
#~ print obj_X
obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1])
#~ print 'obj_Y'
#~ print obj_Y
obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2])
#~ print 'obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z))
obj_pose_orient_norm=obj_pose_orient_norm.transpose()
#~ print 'obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm)
#~ print 'proj'
#~ print proj_vecY
max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir]>0:
hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir]
else:
hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir]
#~ print 'hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec));
angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180.0/np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm=obj_dim[hand_norm_dir]
print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX=np.dot(shelf_X,obj_pose_orient_norm)
max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir]>0:
fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir]
else:
fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir]
#~ print 'fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis=obj_dim[fing_axis_dir]
print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm)
max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ))
Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir]
#~ print 'Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis=obj_dim[Zaxis_dir]
hand_Y=np.cross(hand_norm_vec,fing_axis_vec)
#~ *****************************************************************
#################### DECISION STRATEGIES #########################
bin_inner_cnstr=get_bin_inner_cnstr()
proj_handNorm_ShelfX=np.dot(hand_norm_vec,shelf_X)
right_push=False
left_push=False
# print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm
# print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis
#
# print 'proj_handNorm_ShelfX'
# print proj_handNorm_ShelfX
if obj_dim_along_hand_norm>obj_dim_along_fingAxis:
#~ print 'proj_vecY'
#~ print proj_vecY
#~ print proj_vecY[hand_norm_dir]
#~
#~ print 'proj_vecX'
#~ print proj_vecX
#~ print proj_vecX[fing_axis_dir]
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
#~ else:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
if proj_handNorm_ShelfX>0:
#Do left push
Ypush_mult=1
left_push=True
print '[PushRotate] Left Push'
else:
#Do right push
Ypush_mult=-1
print '[PushRotate] right Push'
right_push=True
push_offset=(obj_dim_along_hand_norm/2.0) #(Instead pf pushing on edge we will push 5 mm inside)
else:
#~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
#~ #Do right push
#~ Ypush_mult=-1
#~ print 'right Push'
#~ else:
#~ #Do left push
#~ Ypush_mult=1
#~ print 'Left Push'
if proj_handNorm_ShelfX>0:
#Do right push
Ypush_mult=-1
print '[PushRotate] right Push'
right_push=True
else:
#Do left push
Ypush_mult=1
print '[PushRotate] Left Push'
left_push=True
push_offset=(obj_dim_along_fingAxis/2.0)
num_intm_points=5
push_x_intm=np.zeros(num_intm_points+1)
push_y_intm=np.zeros(num_intm_points+1)
backWall_shelf=bin_inner_cnstr[binNum][1]
backWall_world = coordinateFrameTransform([0,backWall_shelf,0], 'shelf', 'map', listener)
back_check=backWall_world.pose.position.x-obj_dim_along_hand_norm/2.0
binRightWall=bin_inner_cnstr[binNum][0]
binLeftWall=bin_inner_cnstr[binNum][1]
binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
if left_push:
sideWall_check=binRightWall_world.pose.position.y+(fing_width/2.0)+(obj_dim_along_fingAxis)
if right_push:
sideWall_check=binLeftWall_world.pose.position.y-(fing_width/2.0)-(obj_dim_along_fingAxis)
for i in range(num_intm_points+1):
push_x_intm[i]=obj_pose_pos[0]+(push_offset)*np.sin(((90/num_intm_points)*i*np.pi)/180.0)
if push_x_intm[i]>back_check:
push_x_intm[i]=back_check
push_y_intm[i]=obj_pose_pos[1]+(Ypush_mult*push_offset)*np.cos(((90/num_intm_points)*i*np.pi)/180.0)
if left_push:
if push_y_intm[i]<sideWall_check:
push_y_intm[i]=sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
if right_push:
if push_y_intm[i]>sideWall_check:
push_y_intm[i]=sideWall_check
print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
push_x_intm=np.array(push_x_intm)
#~ print'push_x_intm'
#~ print push_x_intm
push_y_intm=np.array(push_y_intm)
#*******************************************************************
#Move the hand to the center of the bin and open the hand based on the height of the object
bin_mid_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum)
binFloorHeight_world = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener)
#*******************************************************************
tcp_Z_off=0.005
push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z
push_z_intm=(push_tcp_Z)*np.ones(push_x_intm.size)
push_series_pos=np.vstack((push_x_intm,push_y_intm,push_z_intm))
push_series_pos=push_series_pos.transpose()
#~ print 'push_series_pos'
#~ print push_series_pos
#~ print push_series_pos[0,:]
#~ print push_series_pos[-1,:]
push_possible=True
#******************************************************************
fing_push_pt=0.2*obj_dim_along_ZAxis+binFloorHeight_world.pose.position.z
blade_tip_TCP_off=0.038 # dist between finger inner end and spatual edge
push_hand_opening=2*(push_tcp_Z-blade_tip_TCP_off-fing_push_pt)
if push_hand_opening>grasp_range_lim:
push_hand_opening=grasp_range_lim
# Spatula finger should not hit the lip of the bin while pushing
lip_Z_shelf=bin_inner_cnstr[binNum][4]
lip_Z_world=coordinateFrameTransform([0,0,lip_Z_shelf], 'shelf', 'map', listener)
#~ lip_off=.025
#~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off
max_allowed_hand_opening_out=2*(push_tcp_Z-lip_Z_world.pose.position.z)
max_allowed_hand_opening=max_allowed_hand_opening_out-0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface
if push_hand_opening>max_allowed_hand_opening:
print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin'
push_hand_opening=max_allowed_hand_opening
#~ print 'push_hand_opening'
#~ print push_hand_opening
#~ #hand should not hit the side walls
#~ binRightWall,binLeftWall=find_shelf_walls(binNum)
#~
#~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
#~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
side_wall_clearance=0.0
print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y
print '[PushRotate] right_hand_edge=', (push_y_intm[0]-fing_width-side_wall_clearance)
print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y
print '[PushRotate] left_hand_edge=', (push_y_intm[0]+fing_width+side_wall_clearance)
if push_y_intm[0]-fing_width-side_wall_clearance<binRightWall_world.pose.position.y:
print '[PushRotate] Hand will hit the right wall, Push rotate not possible'
push_possible=False
if push_y_intm[0]+fing_width+side_wall_clearance>binLeftWall_world.pose.position.y:
print '[PushRotate] Hand will hit the left wall, push rotate not possible'
push_possible=False
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient=deepcopy(spatula_down_orient)
#~ *************************************************************
if push_possible==True:
#Now we know where we wan to move TCP
#Let's do robot motion planning now
joint_topic = '/joint_states'
# plan store
plans = []
# set tcp (same as that set in generate dictionary)
l1=0.0
l2=0.0
l3 = 0.47
tip_hand_transform = [l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
gripperOri=[0, 0.7071, 0, 0.7071]
pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushRotate] move to bin mouth in push-rotate code successful'
#plan.visualize()
plans.append(plan) # Plan 0
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[PushRotate] move to bin mouth in push-rotate code fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ moveGripper(0.1,100)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1=0.0 #0.035
push_l2=-Ypush_mult*(fing_width/2.0) # This should depend on the righ or left push conditions
push_l3 = 0.47
push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
#~ q_initial = robotConfig
# move just inside the bin with push orientation and open the hand suitable to push
distFromShelf = -0.01
InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [InbinPt.pose.position.x, push_series_pos[0,1] , InbinPt.pose.position.z] #matching world_Y of first push pt
print '[PushRotate] prepush_targetPosition=', prepush_targetPosition
pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)
planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] move inside bin in push orient successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] move inside bin in push orient fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH ###############
# Open the gripper to dim calculated for pushing
print '[PushRotate] hand opening to'
print push_hand_opening*1000.0
#~ moveGripper(push_hand_opening,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
#~ push_rotate_traj = Plan()
#~ push_rotate_traj.q_traj = qf
for i in range(0,push_x_intm.size):
pushpt_pos=push_series_pos[i,:].transpose()
pubFrame(br, pose=pushpt_pos.tolist()+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)
#~ pdb.set_trace()
# planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient,
# joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb)
planner = IK(q0 = q_initial, target_tip_pos = pushpt_pos, target_tip_ori = push_orient,
joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('slow')
s = plan.success()
if s:
print '[PushRotate] point inside push traj successful'
#~ print 'tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushRotate] point inside push traj fail'
return False
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
#~ if isExecute:
#~ pauseFunc(withPause)
#~ push_rotate_traj.execute()
#################### CLOSE THE GRIPPER GRASP #################
#~ moveGripper(0.0,100)
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize()
pauseFunc(withPause)
plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[len(plans)-numOfPlan-1].visualizeBackward()
pauseFunc(withPause)
plans[len(plans)-numOfPlan-1].executeBackward()
#*******************************************************************
print '[PushRotate] push_successful'
return (push_possible,False)
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 push_back(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId='crayola_64_ct',
bin_contents=[
'paper_mate_12_count_mirado_black_warrior', 'crayola_64_ct',
'expo_dry_erase_board_eraser'
],
robotConfig=None,
grasp_range_lim=0.11,
fing_width=0.06,
isExecute=True,
withPause=True):
#~ *****************************************************************
obj_dim = get_obj_dim(objId)
obj_dim = np.array(obj_dim)
#~ *****************************************************************
# # DEFINE HAND WIDTH#########################
hand_width = 0.186
#~ *****************************************************************
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
(shelf_position,
shelf_quaternion) = lookupTransform("map", "shelf", listener)
# print shelf_position, shelf_quaternion
#~ *****************************************************************
# Convert xyx quat to tranformation matrix for Shelf frame
#~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])
shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
shelf_pose_tfm = np.array(shelf_pose_tfm_list)
shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
#~ print '[PushBack] shelf_pose_orient'
#~ print shelf_pose_orient
shelf_pose_pos = shelf_pose_tfm[0:3, 3]
#~ print '[PushBack] shelf_pose_pos'
#~ print shelf_pose_pos
#Normalized axes of the shelf frame
shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])
#~ *****************************************************************
# Convert xyx quat to tranformaation matrix for Object frame
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]
obj_pose_pos = obj_pose_tfm[0:3, 3]
#~ print '[PushBack] obj_pose_orient'
#~ print obj_pose_orient
#Normalized axes of the object frame
obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
#~ print '[PushBack] obj_X'
#~ print obj_X
obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
#~ print '[PushBack] obj_Y'
#~ print obj_Y
obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
#~ print '[PushBack] obj_Z'
#~ print obj_Z
#Normalized object frame
obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
obj_pose_orient_norm = obj_pose_orient_norm.transpose()
#~ print '[PushBack] obj_pose_orient_norm'
#~ print obj_pose_orient_norm
#~ *****************************************************************
# We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
# Find projection of object axes on Y axis of the shelf frame
proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
#~ print '[PushBack] proj'
#~ print proj_vecY
max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
np.fabs(proj_vecY))
if proj_vecY[hand_norm_dir] > 0:
hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
else:
hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]
#~ print '[PushBack] hand_norm_vec'
#~ print hand_norm_vec
#Find angle of the edge of the object
Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
la.norm(hand_norm_vec))
angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180 / np.pi
#~ print'angle_to_shelfY'
#~ print angle_to_shelfY
#Find object dimension along hand normal axis
obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
# print '[PushBack] dim along hand norm'
# print obj_dim_along_hand_norm
#~ *****************************************************************
#Find projection of object axes on X axis of the shelf frame
#To find out which object frame is lying closer to the X axis of the shelf
proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)
max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
np.fabs(proj_vecX))
if proj_vecX[fing_axis_dir] > 0:
fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
else:
fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]
#~ print '[PushBack] fing_axis_vec'
#~ print fing_axis_vec
#Find object dimension along the finger axis
obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
# print '[PushBack] obj_dim_along_fingAxis'
# print obj_dim_along_fingAxis
#~ *****************************************************************
#Find projection of object axes on Z axis of the shelf frame
#To find out which object frame is lying closer to the Z axis of the shelf
proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)
max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
np.fabs(proj_vecZ))
Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]
#~ print '[PushBack] Zaxis_vec'
#~ print Zaxis_vec
#Find object dimension along the finger axis, shelf Z
obj_dim_along_ZAxis = obj_dim[Zaxis_dir]
hand_Y = np.cross(hand_norm_vec, fing_axis_vec)
diag_dist = la.norm(
np.array([obj_dim_along_hand_norm, obj_dim_along_fingAxis]))
# print'req obj diag dist=', diag_dist
#~ *****************************************************************
# Find the maximum distance to which we will push
other_diag = np.zeros(len(bin_contents))
for num_bin_contents in range(0, len(bin_contents)):
if bin_contents[num_bin_contents] != objId:
temp_obj_dim = get_obj_dim(objId)
other_diag[num_bin_contents] = la.norm(
np.array([temp_obj_dim[0], temp_obj_dim[1]]))
max_diag, max_obj_ID = np.max(np.fabs(other_diag)), np.argmax(
np.fabs(other_diag))
# print '[PushBack] max_obj_ID',max_diag
# print '[PushBack] max_obj_ID', bin_contents[max_obj_ID]
#*******************************************************************
bin_inner_cnstr = get_bin_inner_cnstr()
#*******************************************************************
#Check if we can do left push
diag_factor = 0.75
obj_left_edge = obj_pose_pos[1] + diag_factor * diag_dist / 2.0
binLeftWall = bin_inner_cnstr[binNum][1]
binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
'map', listener)
binRightWall = bin_inner_cnstr[binNum][0]
binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
'shelf', 'map', listener)
left_gap = binLeftWall_world.pose.position.y - obj_left_edge
# print '[PushBack] left_gap=', left_gap
fing_clearance = 0.010 # It's like 5 mm on both sides of finger
bin_width = binLeftWall_world.pose.position.y - binRightWall_world.pose.position.y
tolerance_in_push = 0.020
if left_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False, False)
if left_gap > fing_width + fing_clearance:
left_push = True
print '[PushBack] left push is possible'
else:
left_push = False
print '[PushBack] left push is NOT possible'
# check if we can do right push
obj_right_edge = obj_pose_pos[1] - diag_factor * diag_dist / 2.0
right_gap = obj_right_edge - binRightWall_world.pose.position.y
# print '[PushBack] right_gap', right_gap
fing_clearance = 0.010 # It's like 5 mm on both sides of finger
if right_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
return (False, False)
if right_gap > fing_width + fing_clearance:
right_push = True
print '[PushBack] right push is possible'
else:
right_push = False
print '[PushBack] right push is NOT possible'
#*******************************************************************
#Find Push Points for left push
bin_face_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map',
listener)
tcp_Z_off = 0.005 #lower down from center of the bin
push_tcp_Z_shelfFrame = (
(bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
2.0) - tcp_Z_off
push_tcp_Z_WorldFrame = coordinateFrameTransform(
[0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z
push_side_off = 1.0
if left_push:
left_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
left_pushPt_Y = binLeftWall_world.pose.position.y - push_side_off * left_gap
# left_Y_lim=binLeftWall_world.pose.position.y-
left_pushPt_Z = deepcopy(push_tcp_Z)
left_push_Pt = np.array([left_pushPt_X, left_pushPt_Y, left_pushPt_Z])
print '[PushBack] expected left push clearance=', binLeftWall_world.pose.position.y - left_pushPt_Y - fing_width / 2.0
#Find Push Points for right push
if right_push:
right_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
right_pushPt_Y = binRightWall_world.pose.position.y + push_side_off * right_gap
right_pushPt_Z = deepcopy(push_tcp_Z)
right_push_Pt = np.array(
[right_pushPt_X, right_pushPt_Y, right_pushPt_Z])
print '[PushBack] expected right push clearance=', right_pushPt_Y - binRightWall_world.pose.position.y - fing_width / 2.0
#Execute push trajectories
#~ *************************************************************
# set spatual down orientation (rotate wrist)
spatula_down_orient = [0, 0.7071, 0, 0.7071]
push_orient = spatula_down_orient
push_hand_opening = 0.100
joint_topic = '/joint_states'
#~ *************************************************************
#Execure left push trajectory
if left_push:
# plan store
left_plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (left push)'
left_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (left push)'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0
push_l2 = 0.0
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
distFromShelf = -0.015
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, left_push_Pt[1], InbinPt.pose.position.z
] #matching world_Y of first push pt
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
left_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening * 1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
left_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X = bin_face_pos_world.pose.position.x + 0.43
push_stop_worldX = back_bin_X - max_diag - 0.010
left_pushStop_Pt_pos = deepcopy(left_push_Pt)
left_pushStop_Pt_pos[0] = push_stop_worldX
#Push until the end
planner = IK(q0=q_initial,
target_tip_pos=left_pushStop_Pt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
left_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
left_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualize()
pauseFunc(withPause)
left_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(left_plans)):
if isExecute:
left_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
left_plans[len(left_plans) - numOfPlan - 1].executeBackward()
#*******************************************************************
if right_push:
# plan store
right_plans = []
# set tcp (same as that set in generate dictionary)
l1 = 0.0
l2 = 0.0
l3 = 0.47
tip_hand_transform = [
l1, l2, l3, 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)
#~ *************************************************************
# GO TO MOUTH OF THE BIN
q_initial = robotConfig
# move to bin mouth
distFromShelf = 0.1
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
gripperOri = [0, 0.7071, 0, 0.7071]
pubFrame(br,
pose=targetPosition + gripperOri,
frame_id='target_pose',
parent_frame_id='link_6',
npub=5)
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()
plan.setSpeedByName('faster')
s = plan.success()
if s:
print '[PushBack] move to bin mouth in push-back code successful (right push)'
right_plans.append(plan) # Plan 0
else:
print '[PushBack] move to bin mouth in push-back code fail (right push)'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######
# set push_tcp
push_l1 = 0.0
push_l2 = 0.0
push_l3 = 0.47
push_tip_hand_transform = [
l1, l2, l3, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to grasp_tcp
pubFrame(br,
pose=push_tip_hand_transform,
frame_id='push_tip',
parent_frame_id='link_6',
npub=10)
distFromShelf = -0.015
InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
prepush_targetPosition = [
InbinPt.pose.position.x, right_push_Pt[1], InbinPt.pose.position.z
] #matching world_Y of first push pt
pubFrame(br,
pose=prepush_targetPosition + push_orient,
frame_id='target_pose',
parent_frame_id='map',
npub=10)
planner = IK(q0=q_initial,
target_tip_pos=prepush_targetPosition,
target_tip_ori=push_orient,
tip_hand_transform=push_tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] move inside bin in push orient successful'
#~ print '[PushBack] tcp at:'
#~ print(pregrasp_targetPosition)
#~ plan.visualize()
right_plans.append(plan) # Plan 1
#~ if isExecute:
#~ pauseFunc(withPause)
#~ plan.execute()
else:
print '[PushBack] move inside bin in push orient fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#~ *************************************************************
############## OPEN THE GRIPPER To PUSH###############
# Open the gripper to dim calculated for pushing
print '[PushBack] hand opening to'
print push_hand_opening * 1000.0
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
right_plans.append(grasp_plan)
#~ *************************************************************
############ Execute the push trajectory ##############
back_bin_X = bin_face_pos_world.pose.position.x + 0.42
push_stop_worldX = back_bin_X - max_diag - 0.010
right_pushStop_Pt_pos = deepcopy(right_push_Pt)
right_pushStop_Pt_pos[0] = push_stop_worldX
#Push until the end
planner = IK(q0=q_initial,
target_tip_pos=right_pushStop_Pt_pos,
target_tip_ori=push_orient,
joint_topic=joint_topic,
tip_hand_transform=push_tip_hand_transform)
plan = planner.plan()
plan.setSpeedByName('fast')
s = plan.success()
if s:
print '[PushBack] start point push traj successful'
right_plans.append(plan) # Plan 1
else:
print '[PushBack] start point push traj fail'
return (False, False)
qf = plan.q_traj[-1]
q_initial = qf
#################### CLOSE THE GRIPPER GRASP #################
grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
right_plans.append(grasp_plan)
#~ *************************************************************
# #################### EXECUTE FORWARD ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualize()
pauseFunc(withPause)
right_plans[numOfPlan].execute()
# #################### RETREAT ####################
for numOfPlan in range(0, len(right_plans)):
if isExecute:
right_plans[numOfPlan].visualizeBackward()
pauseFunc(withPause)
right_plans[len(right_plans) - numOfPlan - 1].executeBackward()
if right_push or left_push:
push_possible = True
else:
push_possible = False
return (push_possible, 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
