def transformFt2Global(ftlist):
global listener
# transform ft data to global frame
(pos_trasform, ori_trasform) = lookupTransform('base_link', 'link_ft', listener)
rotmat = tfm.quaternion_matrix(ori_trasform)
ft_global = np.dot(rotmat, ftlist + [1.0])
return ft_global[0:3].tolist()
def transformFt2Global(ftlist):
# transform ft data to global frame
(pos_trasform, ori_trasform) = lookupTransform(global_frame_id, '/link_ft',
lr)
rotmat = tfm.quaternion_matrix(ori_trasform)
ft_global = np.dot(rotmat, ftlist + [1.0])
return ft_global[0:3].tolist()
def transformFt2Global(ftlist):
global listener
# transform ft data to global frame
(pos_trasform, ori_trasform) = lookupTransform('base_link', 'link_ft', listener)
rotmat = tfm.quaternion_matrix(ori_trasform)
ft_global = np.dot(rotmat, ftlist + [1.0])
return ft_global[0:3].tolist()
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)
# get the marker pos from vicon
vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg()
rospy.sleep(0.2)
#pause()
try:
vmpos = (
np.array(xyztolist(vmarkers.markers[-1].translation)) /
1000.0).tolist()
except:
print 'vicon pos is bad, not using this data'
continue
# get the marker pos from robot
#(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener)
(vicontrans, rot) = lookupTransform('/map', '/vicon_tip',
listener)
vmpos_robot = list(vicontrans)
# test if the marker is tracked or not, skip
print 'vicon pos %.4f %.4f %.4f' % tuple(
vmpos), 'robot pos %.4f %.4f %.4f' % tuple(vmpos_robot)
if norm(vmpos) < 1e-9:
print 'vicon pos is bad, not using this data'
continue
xs = xs + [vmpos[0]]
ys = ys + [vmpos[1]]
zs = zs + [vmpos[2]]
xt = xt + [vmpos_robot[0]]
yt = yt + [vmpos_robot[1]]
zt = zt + [vmpos_robot[2]]
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [0,0,-(9.40/2/1000+14.15/2/1000),0,0,0,1]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos,ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0,0,0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
print 'box_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.1 move
direc = np.dot(tfm.euler_matrix(0,0,2) , normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7], rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] + (-normal).tolist()[0:2] + [0] + box_pose_des_global[0:3] + box_pose_des_global[6:7] + box_pose_des_global[3:6] + curr_pos.tolist())
index += 1
if len(contact_pts) > limit:
break
if len(contact_pts) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.json', 'w') as outfile:
json.dump({'contact_pts': contact_pts, 'contact_nms': contact_nms}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.mat', {'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
def _detectOneObject(obj_id, bin_num, kinect_num):
global _detect_one_object_srv
global br
print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num
# filter the point cloud
pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num)
if pc is None:
return (None, None)
# string[] model_names
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names'
# ObjectConstraint[] constraints # These apply to all objects
# ---
# SceneHypothesis[] detections
# 2. prepare constraints
bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.2 left
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.3 back
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.4 front
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.5 floor
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.6 top
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.7 on floor
floor_thick = 0.03
ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]-floor_thick/2], trans,rot, pc.header.frame_id), tol, bin_num) )
# string model_name
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# ObjectConstraint[] constraints
# geometry_msgs/Pose true_pose # for testing
# ---
#
# ObjectHypothesis[] detections
with Timer('detect_one_object'):
# detect using capsen
service_name = '/detection_service/detect_one_object'
req = DetectOneObjectRequest()
req.model_name = obj_id
req.cloud = pc
req.constraints = ccnstr
req.foreground_mask = foreground_mask
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)]
print 'Waiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print 'Calling service:', service_name
ret = _detect_one_object_srv(req)
# ret.detections is a list of capsen_vision/ObjectHypothesis
# string name
# geometry_msgs/Pose pose
# float32 score
# float32[] score_components
if len(ret.detections)>0:
print len(ret.detections), 'ObjectHypothesis returned, max score', ret.detections[0].score
for i in range(len(ret.detections)):
poselist_capsen_world = poseTransform(pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener)
cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw
poselist_world = transformBack(cap_T_our, poselist_capsen_world) # transform to our desired pose
# check whether inside bin
poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener)
if inside_bin(poselist_shelf[0:3], bin_num):
#pubFrame(br, poselist_world, 'obj', 'map')
return (poselist_world, ret.detections[i].score)
else:
print 'reject hypo', i, 'because it is outside the target bin'
print 'No ObjectHypothesis satisfy hard bin constraint'
return (None, None)
else:
print 'No ObjectHypothesis returned'
return (None, None)
except:
print 'Calling service:', service_name, 'failed'
print 'encounters errors:', traceback.format_exc()
return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num):
# return pose, retScore
global _detect_objects_srv
global br
global _tflistener
print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num
# 1. filter the point cloud
pc, foreground_mask = get_filtered_pointcloud(obj_ids, bin_num, kinect_num) # need to pass in list
if pc is None or foreground_mask is None:
return (None, None)
# 2. prepare constraints
bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.2 left
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.3 back
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.4 front
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.5 floor
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.6 top
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.7 on floor
floor_thick = 0.03
ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]+floor_thick*2], trans,rot, pc.header.frame_id), tol, bin_num) )
#visualizeConstraint(ccnstr[6], pc.header.frame_id)
#pause()
# 3. detect using capsen
with Timer('detect_objects'):
service_name = '/detection_service/detect_objects'
req = DetectObjectsRequest()
req.model_names = obj_ids
req.constraints = ccnstr
req.cloud = pc
req.foreground_mask = foreground_mask
sum_pt = sum(foreground_mask)
if allFalse(foreground_mask, sum_pt):
return (None, None)
foreground_mask = subsample(foreground_mask, sum_pt)
# outputfile = '/tmp/foreground_mask'
# with open(outputfile, 'w') as outfile:
# json.dump(foreground_mask, outfile)
# pause()
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack
req.find_exact_object_list = True
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
#pdb.set_trace()
try:
print '\tCalling service:', service_name
ret = _detect_objects_srv(req)
# ret.detections is a list of capsen_vision/SceneHypothesis
# [capsen_vision/SceneHypothesis]:
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# capsen_vision/ObjectHypothesis[] objects
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 score
# float32[] score_components
# float32[2] errors
# float32 score
if len(ret.detections)>0:
print '\t', len(ret.detections), 'SceneHypothesis returned, max score', ret.detections[0].score
#print ret.detections
for i in range(len(ret.detections)):
scene = ret.detections[i]
nobj = len(scene.objects)
scene_desired = transformObjectsFromCapsenToDesiredFrame(scene, pc.header.frame_id)
if allObjectsInsideBin(scene_desired, bin_num):
return (scene_desired.objects, scene_desired.score)
#else:
#print 'reject scene hypo', i, 'because one object of it is outside the target bin'
print '\tNo SceneHypothesis satisfy hard bin constraint'
return (None, None)
else:
print '\tNo SceneHypothesis returned'
return (None, None)
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
return (None, None)
def run_explore(filename):
# 0. check if collected or not
dir_base = data_base + "/contour_following/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
if os.path.isfile(jsonfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0] + explore_radius, center_world[1]] + [zup]
setCart(start_pos)
# 2. Rough probing
scan_contact_pts = []
# 2.1 populate start poses for small probing in opposite direction
start_configs = []
jmax = (nstart + 1) // 2
kmax = 2 if nstart > 1 else 1
dth = 2 * np.pi / nstart
for j in xrange(jmax):
for k in xrange(kmax):
i = j + k * (nstart // 2)
start_pos = [
center_world[0] + explore_radius * np.cos(i * dth),
center_world[1] + explore_radius * np.sin(i * dth)
]
direc = [-np.cos(i * dth), -np.sin(i * dth), 0]
start_configs.append([start_pos, direc])
print 'direc', direc
# 2.2 move toward center till contact, and record contact points
for i, (start_pos, direc) in enumerate(reversed(start_configs)):
setZero()
wait_for_ft_calib()
setCart(start_pos + [zup])
setCart(start_pos + [z])
curr_pos = start_pos + [z]
cnt = 0
while True:
curr_pos = np.array(curr_pos) + np.array(direc) * step_size
setCart(curr_pos)
# let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getFTmsglist()
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
print 'obj_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_pt = curr_pos - normal * probe_radius
scan_contact_pts.append(contact_pt.tolist())
if i < len(
start_configs
) - 1: # if the last one, stay in contact and do exploration from there.
setCart([curr_pos[0], curr_pos[1], zup])
break
#if too far and no contact break.
if cnt > explore_radius * 2 / step_size:
break
if len(scan_contact_pts) == 0:
print "Error: Cannot touch the object"
return
# 3. Contour following, use the normal to move along the block
setSpeed(tcp=global_slow_vel)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 3.1 move
direc = np.dot(tfm.euler_matrix(0, 0, 2),
normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos)
# 3.2 let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getAveragedFT()
if index % 50 == 0:
#print index #, '[ft explore]', ft
print index
else:
sys.stdout.write('.')
sys.stdout.flush()
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
# 3.3 visualize it.
vizBlock(box_pose_des_global, obj_mesh, global_frame_id)
# 3.4 record the data if in contact
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radius
contact_pts.append(contact_pt.tolist())
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(),
(contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z.
# Also the normal should point toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] +
(-normal).tolist()[0:2] + [0] +
box_pose_des_global[0:3] +
box_pose_des_global[6:7] +
box_pose_des_global[3:6] +
curr_pos.tolist())
index += 1
# 3.5 break if we collect enough
if len(contact_pts) > limit:
break
# 3.6 periodically zero the ft
if len(
contact_pts
) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=global_super_slow_vel)
setCart(curr_pos + normal * move_away_size)
rospy.sleep(1)
print 'offset ft:', getAveragedFT()
setZero()
wait_for_ft_calib()
setCart(curr_pos - normal * overshoot_size)
setSpeed(tcp=global_slow_vel)
# 4.1 save contact_nm and contact_pt as json file
helper.make_sure_path_exists(dir_base)
with open(jsonfilename, 'w') as outfile:
json.dump(
{
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts,
'isreal': True,
'__title__': colname,
"surface_id": surface_id,
"shape_id": shape_id,
"probe_id": probe_id,
"muc": muc,
"probe_radius": probe_radius,
"offset": offset
},
outfile,
sort_keys=True,
indent=1)
# 4.2 save all_contact as mat file
sio.savemat(matfilename, {
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts
})
# 5. move back to startPos
setSpeed(tcp=global_vel)
start_pos = [curr_pos[0], curr_pos[1]] + [zup]
setCart(start_pos)
recover(obj_slot, 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 main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [
0, 0, -(9.40 / 2 / 1000 + 14.15 / 2 / 1000), 0, 0, 0, 1
]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos, ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0, 0, 0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(
ROS_Wait_For_Msg('/netft_data',
geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
print 'box_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.1 move
direc = np.dot(tfm.euler_matrix(0, 0, 2), normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7],
rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] +
(-normal).tolist()[0:2] + [0] +
box_pose_des_global[0:3] +
box_pose_des_global[6:7] +
box_pose_des_global[3:6] + curr_pos.tolist())
index += 1
if len(contact_pts) > limit:
break
if len(
contact_pts
) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.json',
'w') as outfile:
json.dump({
'contact_pts': contact_pts,
'contact_nms': contact_nms
}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.mat',
{'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
def _detectOneObject(obj_id, bin_num, kinect_num):
global _detect_one_object_srv
global br
print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num
# filter the point cloud
pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num,
kinect_num)
if pc is None:
return (None, None)
# string[] model_names
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names'
# ObjectConstraint[] constraints # These apply to all objects
# ---
# SceneHypothesis[] detections
# 2. prepare constraints
bin_cnstr = get_bin_cnstr(
)[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.2 left
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.3 back
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.4 front
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.5 floor
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.6 top
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.7 on floor
floor_thick = 0.03
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.SUPPORTING_PLANE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] - floor_thick / 2],
trans, rot, pc.header.frame_id), tol, bin_num))
# string model_name
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# ObjectConstraint[] constraints
# geometry_msgs/Pose true_pose # for testing
# ---
#
# ObjectHypothesis[] detections
with Timer('detect_one_object'):
# detect using capsen
service_name = '/detection_service/detect_one_object'
req = DetectOneObjectRequest()
req.model_name = obj_id
req.cloud = pc
req.constraints = ccnstr
req.foreground_mask = foreground_mask
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)]
print 'Waiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print 'Calling service:', service_name
ret = _detect_one_object_srv(req)
# ret.detections is a list of capsen_vision/ObjectHypothesis
# string name
# geometry_msgs/Pose pose
# float32 score
# float32[] score_components
if len(ret.detections) > 0:
print len(
ret.detections
), 'ObjectHypothesis returned, max score', ret.detections[
0].score
for i in range(len(ret.detections)):
poselist_capsen_world = poseTransform(
pose2list(ret.detections[i].pose), pc.header.frame_id,
'map', _tflistener)
cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw
poselist_world = transformBack(
cap_T_our,
poselist_capsen_world) # transform to our desired pose
# check whether inside bin
poselist_shelf = poseTransform(poselist_world, 'map',
'shelf', _tflistener)
if inside_bin(poselist_shelf[0:3], bin_num):
#pubFrame(br, poselist_world, 'obj', 'map')
return (poselist_world, ret.detections[i].score)
else:
print 'reject hypo', i, 'because it is outside the target bin'
print 'No ObjectHypothesis satisfy hard bin constraint'
return (None, None)
else:
print 'No ObjectHypothesis returned'
return (None, None)
except:
print 'Calling service:', service_name, 'failed'
print 'encounters errors:', traceback.format_exc()
return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num):
# return pose, retScore
global _detect_objects_srv
global br
global _tflistener
print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num
# 1. filter the point cloud
pc, foreground_mask = get_filtered_pointcloud(
obj_ids, bin_num, kinect_num) # need to pass in list
if pc is None or foreground_mask is None:
return (None, None)
# 2. prepare constraints
bin_cnstr = get_bin_cnstr(
)[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.2 left
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.3 back
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.4 front
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.5 floor
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.6 top
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.7 on floor
floor_thick = 0.03
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.SUPPORTING_PLANE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] + floor_thick * 2],
trans, rot, pc.header.frame_id), tol, bin_num))
#visualizeConstraint(ccnstr[6], pc.header.frame_id)
#pause()
# 3. detect using capsen
with Timer('detect_objects'):
service_name = '/detection_service/detect_objects'
req = DetectObjectsRequest()
req.model_names = obj_ids
req.constraints = ccnstr
req.cloud = pc
req.foreground_mask = foreground_mask
sum_pt = sum(foreground_mask)
if allFalse(foreground_mask, sum_pt):
return (None, None)
foreground_mask = subsample(foreground_mask, sum_pt)
# outputfile = '/tmp/foreground_mask'
# with open(outputfile, 'w') as outfile:
# json.dump(foreground_mask, outfile)
# pause()
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack
req.find_exact_object_list = True
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
#pdb.set_trace()
try:
print '\tCalling service:', service_name
ret = _detect_objects_srv(req)
# ret.detections is a list of capsen_vision/SceneHypothesis
# [capsen_vision/SceneHypothesis]:
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# capsen_vision/ObjectHypothesis[] objects
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 score
# float32[] score_components
# float32[2] errors
# float32 score
if len(ret.detections) > 0:
print '\t', len(
ret.detections
), 'SceneHypothesis returned, max score', ret.detections[
0].score
#print ret.detections
for i in range(len(ret.detections)):
scene = ret.detections[i]
nobj = len(scene.objects)
scene_desired = transformObjectsFromCapsenToDesiredFrame(
scene, pc.header.frame_id)
if allObjectsInsideBin(scene_desired, bin_num):
return (scene_desired.objects, scene_desired.score)
#else:
#print 'reject scene hypo', i, 'because one object of it is outside the target bin'
print '\tNo SceneHypothesis satisfy hard bin constraint'
return (None, None)
else:
print '\tNo SceneHypothesis returned'
return (None, None)
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
return (None, None)
setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th)
# get the marker pos from vicon
vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg()
rospy.sleep(0.2)
#pause()
try:
vmpos = (np.array(xyztolist(vmarkers.markers[-1].translation)) / 1000.0).tolist()
except:
print 'vicon pos is bad, not using this data'
continue
# get the marker pos from robot
#(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener)
(vicontrans,rot) = lookupTransform('/map','/vicon_tip', listener)
vmpos_robot = list(vicontrans)
# test if the marker is tracked or not, skip
print 'vicon pos %.4f %.4f %.4f' % tuple(vmpos), 'robot pos %.4f %.4f %.4f' % tuple(vmpos_robot)
if norm(vmpos) < 1e-9:
print 'vicon pos is bad, not using this data'
continue
xs = xs + [vmpos[0]]
ys = ys + [vmpos[1]]
zs = zs + [vmpos[2]]
xt = xt + [vmpos_robot[0]]
yt = yt + [vmpos_robot[1]]
zt = zt + [vmpos_robot[2]]
print 'data length', len(xs)
setAcc(acc=globalacc, deacc=globalacc)
for x in np.linspace(limits[0], limits[1], nseg[0]):
for y in np.linspace(limits[2], limits[3], nseg[1]):
for z in np.linspace(limits[4], limits[5], nseg[2]):
setCart([x, y, z], ori)
js = getJoint()
#raw_input('pause') # to see where is bad
for th in np.linspace(-180, 180, nrotate):
setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th)
# get the marker pos from vicon
rospy.sleep(0.2)
(vmarkerstrans, rot) = lookupTransform(
'/viconworld', '/vicon/CalibViconPlate/CalibViconPlate',
listener)
#pause()
try:
vmpos = list(vmarkerstrans)
except:
print 'vicon pos is bad, not using this data'
continue
# get the marker pos from robot
#(vicontrans,rot) = lookupTransform('/viconworld','/vicon_tip', listener)
(vicontrans, rot) = lookupTransform('/map', '/vicon_tip',
listener)
vmpos_robot = list(vicontrans)
# test if the marker is tracked or not, skip
