def graspGripper(move_pos, move_speed=50):
# move_pos in meter, move_speed in mm/s
#WE should make grasp gripper force controlled--Nikhil
# call gripper node, grasp
move_pos=1000*move_pos
gripper.move(move_pos, move_speed)
return 1
def graspGripper(move_pos, move_speed=50):
# move_pos in meter, move_speed in mm/s
# WE should make grasp gripper force controlled--Nikhil
# call gripper node, grasp
move_pos = 1000 * move_pos
gripper.move(move_pos, move_speed)
return 1
def moveGripper(move_pos, move_speed=50):
#~publis gripper state
jnames = ['gripper_command']
gripper_msgs = sensor_msgs.msg.JointState()
gripper_msgs.name = jnames
gripper_msgs.position = [move_pos]
gripper_msgs.velocity = [move_speed / 1000]
gripper_msgs.effort = [np.inf]
gripper_command_pub.publish(gripper_msgs)
# move_pos in meter, move_speed in mm/s
# call gripper node, grasp
move_pos = 1000 * move_pos
gripper.move(move_pos, move_speed)
return 1
def run_grasping(self, container=None):
self.getBestGraspingPoint(container)
#~Publish grasp proposal information
grasp_proposal_msgs = Float32MultiArray()
grasp_proposal_msgs.data = self.grasp_point
if self.grasp_point is not None:
self.grasp_proposal_pub.publish(grasp_proposal_msgs)
comments_msgs = String()
comments_msgs.data = self.experiment_description
experiment_type_msgs = String()
experiment_type_msgs.data = self.experiment_type
self.experiment_comments_pub.publish(comments_msgs)
self.experiment_type_pub.publish(experiment_type_msgs)
else:
print('There are no grasp proposal')
self.execution_possible = False
return
#~Visualize grasp proposals
markers_msg = MarkerArray()
m0 = createDeleteAllMarker('pick_proposals')
markers_msg.markers.append(m0)
for i in range(10):
self.proposal_viz_array_pub.publish(markers_msg)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, self.all_grasp_proposals,
self.listener, self.br)
ik.visualize_helper.visualize_grasping_proposals(
self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
self.listener, self.br, True)
#execute for grasp. Stop when the gripper is closed
self.back_img_list = self.capture_images()
#self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]
self.grasping_output = grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr)
num_it = 0
print('gripper open: ', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
while is_in_wrong_pose:
'''
self.retrieve_output = retrieve(listener=self.listener, br=self.br,
isExecute=self.isExecute, binId=container,
withPause=self.withPause, viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr, ws_object=self.weightSensor, isShake=False)
self.grasping_output = grasp(objInput=self.grasp_point, listener=self.listener, br=self.br,
isExecute=self.isExecute, binId=container, withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub, recorder=self.gdr, open_hand=False)
'''
# Motion heuristic
initial_dz = 0.05
dz = .022 #should be related to object length
ik.helper.move_cart(dz=initial_dz)
rospy.sleep(0.5)
ik.helper.move_cart(dz=-initial_dz)
rospy.sleep(0.5)
gripper.move(90)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
#should be done in the direction of the gripper plane
dx = 0.038 #.02*num_it
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
print('gripper_open', gripper.getGripperopening())
is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
if is_in_wrong_pose:
gripper.move(90)
rospy.sleep(0.5)
ik.helper.move_cart(dz=dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=-dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
ik.helper.move_cart(dz=-dz)
rospy.sleep(0.5)
ik.helper.move_cart_hand(self.listener,
dx=dx,
dy=0,
dz=0,
speedName='fastest')
rospy.sleep(0.5)
gripper.close()
rospy.sleep(0.5)
self.gdr.save_data_recorded = False
num_it += 1
is_in_wrong_pose = (
gripper.getGripperopening() < 0.04
) #and (gripper.getGripperopening() > 0.015)
self.retrieve_output = retrieve(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=container,
withPause=self.withPause,
viz_pub=self.proposal_viz_array_pub,
recorder=self.gdr,
ws_object=self.weightSensor,
isShake=False)
self.execution_possible = self.retrieve_output['execution_possible']
def flush_grasp(objPose = [1.95,1.25,1.4,0,0,0,1],
binNum=4,
objId = 0,
bin_contents = None,
robotConfig = None,
forceThreshold = 1,
isExecute = True,
withPause = False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
#tcp_offset_variables
#set robot speed
setSpeedByName(speedName = 'fast')
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
obj_pose_tfm=np.array(obj_pose_tfm_list)
obj_pose_orient=obj_pose_tfm[0:3,0:3]
vertical_index=np.argmax(np.fabs(obj_pose_orient[2,0:3]))
object_dim=get_obj_dim(objId)
vertical_dim=object_dim[vertical_index]
s1=np.fabs(obj_pose_orient[1,0])*object_dim[0]
s2=np.fabs(obj_pose_orient[1,1])*object_dim[1]
s3=np.fabs(obj_pose_orient[1,2])*object_dim[2]
s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index]
horizontal_dim=s1+s2+s3-s4
hand_gap=0
gripper.close()
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
vert_offset=.035
l2 = 0.43 #change this to edit where you think the cup is
l2 =.44
tip_hand_transform = [-vert_offset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# for i in range(5):
# rospy.sleep(0.1)
# br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
#this may cause issues!!!
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth,bin_height,bin_width) = getBinMouth(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
object_depth=objPosition[0]-binMouth[0]
finger_length=.23
finger_width=.06
up_offset=.05
down_offset=0.005
cup_to_spatula=.08
hand_width=.15
max_gripper_width=.110
hand_top_offset=hand_width/2+vert_offset+.03
hand_bot_offset=hand_width/2-vert_offset+.015
side_offset=.03
binFloorHeight=binMouth[2]-bin_height/2
binCeilHeight=binMouth[2]+bin_height/2
min_height=binFloorHeight+lipHeight+finger_width/2+down_offset
desired_height=objPosition[2]
max_height=binCeilHeight-lipHeight-finger_width/2-down_offset
target_height=desired_height
if target_height<min_height:
target_height=min_height
if target_height>max_height:
target_height=max_height
bin_sideways=binMouth[1]
sidepos=objPosition[1]
horz_offset=0.015
#this determines bin sides based on bin input
binSmall=binMouth[1]-bin_width/2
binLarge=binMouth[1]+bin_width/2
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
binRight=binSmall
binLeft=binLarge
final_hand_gap=110
if sidepos>bin_sideways:
#this stuff is what happens if the object is to the left
print 'Object is to the left'
if binNum==0 or binNum==3 or binNum==6 or binNum==9:
print 'Object is in that weird rail corner'
return False
#turn the suction cup so that it is sideways left
#scoopOrientation = [.5,-.5,.5,-.5]
scoopOrientation = [0.5,.5,.5,.5]
#side_waypoint1=binRight+cup_to_spatula+.01
#side_waypoint2=sidepos+horizontal_dim-side_offset
#side_waypoint2=binLeft-horizontal_dim-horz_offset
side_waypoint1=binLeft-hand_top_offset+horz_offset
if sidepos-horizontal_dim<binLeft-final_hand_gap:
print 'this is not a flush object'
return False
else:
#this stuff is what happens if the object is to the right
print 'Object is to the right'
if binNum==2 or binNum==5 or binNum==8 or binNum==11:
print 'Object is in that weird rail corner'
return False
#turn the suction cup so that it is sideways right
#scoopOrientation = [0.5,.5,.5,.5]
scoopOrientation = [.5,-.5,.5,-.5]
#side_waypoint1=binLeft-cup_to_spatula-.01
#side_waypoint2=sidepos-horizontal_dim+side_offset
#side_waypoint2=binRight+horizontal_dim+horz_offset
side_waypoint1=binRight+hand_top_offset-horz_offset
if sidepos+horizontal_dim>binRight+final_hand_gap:
print 'this is not a flush object'
return False
targetPositionList=[
[binMouth[0]-.15, side_waypoint1, min_height],
[binMouth[0]+.04, side_waypoint1, min_height],
[binMouth[0]+.3, side_waypoint1, min_height],
[binMouth[0]+.3, side_waypoint1, min_height+.03],
[binMouth[0]-.1, side_waypoint1, min_height+.03]]
qf=q0
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
frontOfObjectPtOutOfBin = targetPosition
q_initial = qf
planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print 'move to COM in y successful'
print 'tcp at:'
print(targetPosition)
#plan.visualize(hand_param=hand_gap)
plans.append(plan)
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print 'move to COM in y fail'
return False
#print plan.q_traj
qf = plan.q_traj[-1]
print qf
for numOfPlan in range(0, 2):
if isExecute:
plans[numOfPlan].visualize(hand_param=final_hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
#time.sleep(2.5)
#suction.start()
final_hand_gap=110
gripper.set_force(20)
gripper.grasp(move_pos=final_hand_gap)
for numOfPlan in range(2, 3):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
gripper.set_force(50)
rospy.sleep(0.1)
gripper.move(move_pos=0)
hand_gap=final_hand_gap
print hand_gap
## retreat
#for numOfPlan in range(0, len(plans)-1):
# plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
# if isExecute:
# pauseFunc(withPause)
# plans[len(plans)-numOfPlan-1].executeBackward()
# suction.stop()
for numOfPlan in range(3, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
return True
def moveGripper(move_pos, move_speed=50):
# move_pos in meter, move_speed in mm/s
# call gripper node, grasp
move_pos=1000*move_pos
gripper.move(move_pos, move_speed)
return 1
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 moveGripper(move_pos, move_speed=50):
# move_pos in meter, move_speed in mm/s
# call gripper node, grasp
move_pos = 1000 * move_pos
gripper.move(move_pos, move_speed)
return 1