def goToHomeSuction(plan=None):
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
plan_n = Plan()
if q0[5] < 0:
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518 - 2 * math.pi
]
else:
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518
]
plan_n.q_traj = [possible_start_config]
return plan_n
def getAveragedFT():
tmpft = np.array([0,0,0])
nsample = 100
for i in xrange(0,nsample):
tmpft = tmpft + np.array(ftmsg2list(ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg()))
#print tmpft / nsample
return (tmpft / nsample).tolist()
def getAveragedFT():
tmpftsum = np.array([0, 0, 0, 0, 0, 0])
nsample = 200
tmpftlist = []
for i in xrange(0, nsample):
tmpft = ftmsg2list(
ROS_Wait_For_Msg('/netft_data',
geometry_msgs.msg.WrenchStamped).getmsg())
tmpftlist.append(tmpft)
tmpftsum = tmpftsum + np.array(tmpft)
#print tmpft / nsample
return (tmpftsum / nsample).tolist(), np.std(tmpftlist, axis=0)
def wait_for_ft_calib():
ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg()
setSpeed(100, 60)
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]):
# import pdb;pdb.set_trace()
setCart([x, y, z], ori)
js = getJoint()
for th in np.linspace(-180, 180, nrotate):
# import pdb;pdb.set_trace()
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)
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 push(start_pos, end_pos):
global frame_work_robot_2d, zup, z
global contact_pts, contact_nms, all_contact
global step_size
direc = (npa(end_pos) - npa(start_pos))
dist = np.linalg.norm(direc)
direc = direc / dist * step_size
setSpeed(tcp=global_slow_vel)
setCart(list(start_pos) + [zup])
# move down, not supposed to make contact
setCart(list(start_pos) + [z])
nsteps = int(dist // step_size)
# calibrate the FT sensor
#setZero()
#wait_for_ft_calib()
for i in xrange(1, nsteps + 1):
# move
curr_pos = (direc * i + start_pos).tolist() + [z]
setCart(curr_pos)
# collect vicon object pose
box_pose_des_global = lookupTransformList(global_frame_id,
obj_frame_id, lr)
# collect ft measurement
rospy.sleep(sleepForFT)
ft = getAveragedFT()
# collect apriltag measurement
rospy.sleep(sleepForAP)
has_apriltag = False
object_apriltag = [0, 0, 0, 0, 0, 0, 0]
apriltag_detections_msg = ROS_Wait_For_Msg(
'/apriltags/detections', AprilTagDetections).getmsg()
for det in apriltag_detections_msg.detections:
if det.id == 0:
pose_webcam = transform_back([0.02, 0.02, 0.009, 0, 0, 0, 1],
pose2list(det.pose))
object_apriltag = poseTransform(pose_webcam, '/head_camera',
'/map', lr)
has_apriltag = True
break
# compute contact point and normal if in contact
if norm(ft[0:2]) > threshold:
incontact = True
# 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.8).tolist())
else:
incontact = False
normal = npa([0, 0, 0])
contact_pt = npa([0, 0, 0])
# record robot pos, object pose, contact point, contact normal, and ft measurement
# 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 + [incontact] +
object_apriltag[0:3] + object_apriltag[6:7] +
object_apriltag[3:6] +
[pose3d_to_pose2d(object_apriltag)[2]] +
[has_apriltag])
setCart((direc * nsteps + start_pos).tolist() + [zup])
def wait_for_ft_calib():
from ik.roshelper import ROS_Wait_For_Msg
ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg()
def scoop(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
binNum=3,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9116, -0.012498, -0.4971],
forceThreshold=1,
isExecute=True,
withPause=True,
withVisualize=False):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
setSpeedByName(speedName='faster')
joint_topic = '/joint_states'
planSuccess = True
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
if binNum < 3:
pretensionDelta = 0.03
if binNum > 2 and binNum < 6:
pretensionDelta = 0.009
if binNum > 5 and binNum < 9:
pretensionDelta = 0.009
if binNum > 8:
pretensionDelta = 0.03
#pretensionDelta = 0.00
lipHeight = 0.025
# plan store
plans = []
## get object position (world frame)
objPosition = getObjCOM(objPose[0:3], objId)
## move gripper to object com outside bin along world y direction and
## move the gripper over the lip of the bin
# set tcp
tcpXOffset = 0.018
l2 = 0.47
tip_hand_transform = [
tcpXOffset, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
scoopOrientation = [0, 0.7071, 0, 0.7071]
# set first target to move gripper in front of the object and adjust height to middle of bin
distFromShelf = 0.05
wristWidth = 0.0725 # this is actually half the wrist width
(binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
listener)
binMouth = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
verticalOffsetLip = 0.00 # we need this so we don't damage the sucker
wH = 0.075
targetPosition = [
binMouth[0], objPosition[1],
binMouth[2] + tcpXOffset - wH + lipHeight - pretensionDelta
]
## check to make sure we are inside the bin and not colliding with sides:
minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum)
leftWall = coordinateFrameTransform([leftWall, 0, 0], 'shelf', 'map',
listener)
leftWall = leftWall.pose.position.y
rightWall = coordinateFrameTransform([rightWall, 0, 0], 'shelf', 'map',
listener)
rightWall = rightWall.pose.position.y
interiorLipBin = [0, 0.40,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map',
listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.20
sStroke = 0.19
binLengthSafety = 0.015
if targetPosition[1] + 0.04 > leftWall:
interiorLipBin = [0, 0.36,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] - 0.04 < leftWall:
interiorLipBin = [0, 0.36,
0] # define over the lip distance in shelf frame
interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
'map', listener)
stepOverLip = interiorLipBin.pose.position.x
fStroke = 0.17
if targetPosition[1] + binLengthSafety > leftWall:
targetPosition[1] = leftWall
if targetPosition[1] - binLengthSafety < rightWall:
targetPosition[1] = rightWall
frontOfObjectPtOutOfBin = targetPosition
q_initial = robotConfig
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success() # Plan 0
if s:
print '[Scoop] move to COM in y successful'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
visualizeFunc(withVisualize, plan)
plans.append(plan)
else:
print '[Scoop] move to COM in y fail'
return (False, False)
qf = plan.q_traj[-1]
## push spatula against base of bin (pre-tension)
binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf',
'map', listener)
q_initial = qf
percentTilt = 0.1 # 10 percent
scoopOrientation = [
0, 0.7071 * (1 + percentTilt), 0, 0.7071 * (1 - percentTilt)
]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success() # Plan 1
if s:
print '[Scoop] reorient the hand'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] reorient the hand fail'
return (False, False)
qf = plan.q_traj[-1]
# set second target, go over the lip of the bin
deltaX = np.add(-targetPosition[0], stepOverLip)
targetPosition = np.add(targetPosition, [deltaX, 0, 0])
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success() # Plan 2
if s:
print '[Scoop] move to inside the lip success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] move to inside the lip fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to middle
q_initial = qf
targetPosition = np.add(targetPosition, [fStroke, 0, -lipHeight])
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success() # Plan 3
if s:
print '[Scoop] stroke middle of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## perform bin length stroke to end
q_initial = qf
targetPosition = np.add(targetPosition, [sStroke, 0, 0])
scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success() # Plan 4
if s:
print '[Scoop] stroke middle to end of bin success'
#~ print '[Scoop] tcp at:'
#~ print(targetPosition)
plans.append(plan)
visualizeFunc(withVisualize, plan)
else:
print '[Scoop] stroke middle to end of bin fail'
return (False, False)
qf = plan.q_traj[-1]
## close gripper
#~ closeGripper(forceThreshold)
closeGripper(forceThreshold)
execution_possible = True
## execute
isNotInCollision = True
for numOfPlan in range(0, len(plans)):
plans[numOfPlan].visualize()
if isExecute:
if numOfPlan == 3:
openGripper()
pauseFunc(withPause)
isNotInCollision = plans[numOfPlan].execute()
if numOfPlan == 3:
closeGripper(forceThreshold)
plans[numOfPlan].executeBackward()
openGripper()
plans[numOfPlan].execute()
if not isNotInCollision:
planFailNum = numOfPlan
print '[Scoop] collision detected'
break
if numOfPlan == 4:
closeGripper(forceThreshold)
rospy.sleep(0.5)
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) == 2 or len(q0) == 8:
q0 = q0[-2:] # get last 2
break
gripper_q0 = np.fabs(q0)
drop_thick = 0.000001 # finger gap =0.002m = .5 mm
if gripper_q0[0] < drop_thick:
print '[Scoop] ***************'
print '[Scoop] Could not grasp'
print '[Scoop] ***************'
execution_possible = False
else:
print '[Scoop] ***************'
print '[Scoop] Grasp Successful'
print '[Scoop] ***************'
execution_possible = True
if not isNotInCollision:
for numOfPlan in range(0, len(planFailNum)):
plans[planFailNum - numOfPlan].executeBackward()
## retreat
for numOfPlan in range(0, len(plans)):
if withVisualize:
plans[len(plans) - numOfPlan - 1].visualizeBackward()
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
return True, execution_possible
def goToBin(binNum=0,
robotConfig=None,
objectiveBinPos=[1.2, 0, 0.6],
isExecute=True,
withPause=False,
withSuction=False,
counter=0):
## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
## objId: object identifier
## robotConfig: current time robot configuration
## shelfPosition: shelf position in world frame
## force threshold: amount fo force needed to say we have a grasp
planSuctionSuccess = False
planGraspSuccess = False
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [
0, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
# rospy.sleep(0.1)
# br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
# rospy.sleep(0.1)
pubFrame(br,
pose=tip_hand_transform,
frame_id='target_pose',
parent_frame_id='map',
npub=1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
# set scoop orientation (rotate wrist)
if not withSuction:
gripperOri = [0, 0.7071, 0, 0.7071]
if withSuction:
print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
gripperOri = [
pose_world.pose.orientation.x, pose_world.pose.orientation.y,
pose_world.pose.orientation.z, pose_world.pose.orientation.w
]
# move back 10 cms to avoid object with shelf collision
distAwayFromBin = 0.1
targetPosition = [tcpPos[0] - distAwayFromBin, tcpPos[1], tcpPos[2]]
q_initial = robotConfig
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
print '[goToBin] tcp at:', targetPosition
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] move away from bin fail'
return False
qf = plan.q_traj[-1]
if binNum > 8:
#plan = Plan()
#plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]] # should use IKJoint
planner = IKJoint(
q0=qf,
target_joint_pos=[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745])
plan = planner.plan()
print '[goToBin] going home because risky to go straight to objective bin'
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
# move above objective bin, approx
use_JointPos = False
if use_JointPos:
planner = IKJoint(
q0=qf, target_joint_pos=[0, -0.2953, 0.4462, 0, 0.8292, 1.5707])
plan = planner.plan()
#plan = Plan()
#plan.q_traj = [[0, -0.2953, 0.4462, 0, 0.8292, 1.5707]] # should use IKJoint
s = True
else:
if not withSuction:
gripperOri = [0.7071, 0.7071, 0, 0]
locationAboveBinCOM = [
0, 0, 0.15
] # define over the lip distance in shelf frame
targetPosition = [
objectiveBinPos[0] + locationAboveBinCOM[0],
objectiveBinPos[1] + locationAboveBinCOM[1],
objectiveBinPos[2] + locationAboveBinCOM[2]
]
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] move to above bin success'
planSuctionSuccess = True
plans.append(plan)
plan.visualize()
qf = plan.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
return False
if withSuction:
bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map',
listener)
targetPosition = [
bin_xyz.pose.position.x, bin_xyz.pose.position.y,
bin_xyz.pose.position.z
]
q_initial = qf
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[goToBin] setting joints for suction'
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) < 6:
continue
else:
break
if q0[5] < 0:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834,
3.1415 - 2 * math.pi
]
else:
stackAngle = -(counter -
1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
possible_start_config = [
stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415
]
# plan_n = Plan()
# plan_n.q_traj=[possible_start_config]
planner = IKJoint(target_joint_pos=possible_start_config)
plan_n = planner.plan()
plan_n.visualize()
qf = plan_n.q_traj[-1]
if isExecute:
pauseFunc(withPause)
plan_n.execute()
## go down and release object
if not withSuction:
q_initial = qf
objectiveBinPos[1] = (counter - 1) * 0.036 - 0.2
targetPosition = [
objectiveBinPos[0], objectiveBinPos[1], objectiveBinPos[2]
]
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToBin] go to xyz of bin with vertical bias success'
plans.append(plan)
plan.visualize()
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToBin] go to xyz of bin with vertical bias fail'
qf = plan.q_traj[-1]
openGripper()
## open gripper fully
# rospy.sleep(0.5)
# openGripper()
rospy.sleep(0.5)
suction.stop()
return True
def suction_down(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
forceThreshold=1,
isExecute=True,
withPause=True):
## 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()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
## 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)
object_dim = adjust_obj_dim(objId, object_dim)
vertical_dim = object_dim[vertical_index]
while True:
APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
sensor_msgs.msg.JointState).getmsg()
q0 = APCrobotjoints.position
if len(q0) >= 6:
q0 = q0[
0:
6] # take first 6, because in virtual environmet there will be additional 2 hand joint
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 = .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
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)
scoopOrientation = [0.7071, 0, 0.7071, 0]
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
]
hand_gap = 0
gripper.close()
finger_length = .23
finger_width = .08
up_offset = .05
down_offset = -.025
cup_to_spatula = .08
hand_width = .15
max_gripper_width = 110
hand_top_offset = hand_width / 2 + vert_offset + .01
hand_bot_offset = hand_width / 2 - vert_offset
#this determines bin stuff based on bin input
binFloorHeight = binMouth[2] - bin_height / 2
binCeilHeight = binMouth[2] + bin_height / 2
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall, binLarge) = getSides(binNum, listener)
horz_offset = 0
small_limit = binSmall + finger_width / 2 + horz_offset
large_limit = binLarge - finger_width / 2 - horz_offset
plans = []
plan = Plan()
if objPosition[1] < 0:
link6_angle = (q0[5] - math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518 - 2 * math.pi
]
else:
link6_angle = (q0[5] + math.pi)
possible_start_config = [
0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
0.09102420595, 3.33888627518
]
#start_config=possible_start_config
start_config = [q0[0], q0[1], q0[2], q0[3], q0[4], link6_angle]
planner = IKJoint(q0=q0, target_joint_pos=start_config)
plan = planner.plan()
#plan.q_traj=[q0,start_config]
plans.append(plan)
qf = plan.q_traj[-1]
def get_motion_param(target_x, target_y):
object_depth = target_x - binMouth[0]
sidepos = min(max(target_y, small_limit), large_limit)
if object_depth < finger_length:
print '[SucDown] shallow suction'
h1 = binCeilHeight - lipHeight - cup_to_spatula
h2 = binFloorHeight + vertical_dim - down_offset
else:
print '[SucDown] deep suction, quitting'
h1 = binCeilHeight - lipHeight - hand_top_offset
h2a = binFloorHeight + vertical_dim - down_offset
h2b = binFloorHeight + hand_bot_offset + lipHeight
h2 = max(h2a, h2b)
return False, h1, h2, sidepos
h2 = max(h2, binFloorHeight)
if h2 > h1:
print '[SucDown] cant go in'
return False, h1, h2, sidepos
return True, h1, h2, sidepos
def generate_plan(targetPositionList, plans, qf):
for tp_index in range(0, len(targetPositionList)):
targetPosition = targetPositionList[tp_index]
planner = IK(q0=qf,
target_tip_pos=targetPosition,
target_tip_ori=scoopOrientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
print '[SucDown] Plan number:', tp_index + 1
if s:
print '[SucDown] Plan calculated successfully'
plans.append(plan)
else:
print '[SucDown] Plan calulation failed'
return (False, plans, qf)
qf = plan.q_traj[-1]
return (True, plans, qf)
def execute_forward(plans, hand_gap):
for numOfPlan in range(0, len(plans)):
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
def execute_backward(plans, hand_gap, plan_offset):
for numOfPlan in range(0, len(plans) - plan_offset):
plans[len(plans) - numOfPlan -
1].visualizeBackward(hand_param=hand_gap)
if isExecute:
pauseFunc(withPause)
plans[len(plans) - numOfPlan - 1].executeBackward()
def try_suction(target_x, target_y, plans, qf, num_iter):
(continue_val, h1, h2, sidepos) = get_motion_param(target_x, target_y)
if continue_val == False:
return False, False
final_hand_gap = max_gripper_width
targetPositionList = [[target_x, sidepos, h1], [target_x, sidepos, h2]]
(continue_val, plans,
throwaway) = generate_plan(targetPositionList, plans, qf)
if continue_val == False:
return False, False
#set robot speed
setSpeedByName(speedName='faster')
hand_gap = 0
gripper.close()
execute_forward(plans, hand_gap)
#suction.start()
gripper.set_force(10)
gripper.grasp(move_pos=max_gripper_width)
gripper.close()
hand_gap = max_gripper_width
print '[SucDown] hand_gap:', hand_gap
execute_backward(plans, hand_gap, 0)
#time.sleep(4)
my_return = suction.check() or suction_override(objId)
if my_return == True:
#set robot speed
setSpeedByName(speedName='fast')
return (True, True)
else:
#suction.stop()
return (True, False)
target_offset = get_test_offset(objId)
target_x_list = [
objPosition[0], objPosition[0] - target_offset, objPosition[0],
objPosition[0], objPosition[0] + target_offset
]
target_y_list = [
objPosition[1], objPosition[1], objPosition[1] + target_offset,
objPosition[1] - target_offset, objPosition[1]
]
count = 0
suction_succeed = False
overall_plan_succeed = False
(continue_val, h1, h2,
sidepos) = get_motion_param(target_x=target_x_list[0],
target_y=target_y_list[0])
if continue_val == False:
return False, False
#targetPositionList=[
#[binMouth[0]-.15, sidepos, h1],
#[binMouth[0]-.15, sidepos, h1],
#[target_x_list[0], sidepos, h1]]
targetPositionListA = [[binMouth[0] - .15, sidepos, h1],
[binMouth[0] - .15, sidepos, h1]]
targetPositionListB = [[target_x_list[0], sidepos, h1]]
(continue_val, plans1, qf) = generate_plan(targetPositionListA, plans, qf)
if continue_val == False:
return False, False
(continue_val, plans2, qf) = generate_plan(targetPositionListB, [], qf)
if continue_val == False:
return False, False
setSpeedByName(speedName='yolo')
execute_forward(plans1, 0)
setSpeedByName(speedName='faster')
execute_forward(plans2, 0)
suction.start()
#time.sleep(6)
#plans_store=plans
plans = []
for count in range(0, len(target_x_list)):
(plan_succeed,
suction_succeed) = try_suction(target_x=target_x_list[count],
target_y=target_y_list[count],
plans=plans,
qf=qf,
num_iter=count)
if suction_succeed:
overall_plan_succeed = True
break
#return (plan_succeed,suction_succeed)
if plan_succeed == True:
overall_plan_succeed = True
while True:
APCrobotjoints = ROS_Wait_For_Msg(
joint_topic, sensor_msgs.msg.JointState).getmsg()
qf = APCrobotjoints.position
if len(qf) < 6:
continue
else:
break
print '[SucDown] qf:', hand_gap
plans = []
count = count + 1
execute_backward(plans2, 0, 0)
if not suction_succeed:
suction.stop()
return (overall_plan_succeed, suction_succeed)
def suction_side(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
binNum=4,
objId=0,
bin_contents=None,
robotConfig=None,
shelfPosition=[1.9019, 0.00030975, -0.503],
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()
#rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# shelf variables
pretensionDelta = 0.00
lipHeight = 0.035
#tcp_offset_variables
# 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='target_pose',
parent_frame_id='tip',
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
]
#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 = .08
up_offset = .05
down_offset = .01
cup_to_spatula = .08
hand_width = .15
max_gripper_width = .110
hand_top_offset = hand_width / 2 + vert_offset + .015
hand_bot_offset = hand_width / 2 - vert_offset + .015
up_down_adjust = .025
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 - up_down_adjust
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.01
#this determines bin sides based on object
#(binSmall,binLarge)=getSides(objPosition[1],listener)
(binSmall, binLarge) = getSides(binNum, listener)
binRight = binSmall
binLeft = binLarge
if sidepos > bin_sideways:
#this stuff is what happens if the object is to the left
print '[SucSide] Object is to the left'
#turn the suction cup so that it is sideways left
scoopOrientation = [.5, -.5, .5, -.5]
side_waypoint1a = binRight + cup_to_spatula + .01
side_waypoint1b = sidepos - horizontal_dim / 2 - 2 * horz_offset
if side_waypoint1a > side_waypoint1b:
side_waypoint1 = side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1 = side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos+horizontal_dim-side_offset
side_waypoint2 = binLeft - horizontal_dim - horz_offset
if side_waypoint2 < side_waypoint1:
if side_waypoint2 < side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1 = side_waypoint2
else:
#this stuff is what happens if the object is to the right
print '[SucSide] Object is to the right'
#turn the suction cup so that it is sideways right
scoopOrientation = [0.5, .5, .5, .5]
side_waypoint1a = binLeft - cup_to_spatula - .01
side_waypoint1b = sidepos + horizontal_dim / 2 + 2 * horz_offset
if side_waypoint1a < side_waypoint1b:
side_waypoint1 = side_waypoint1a
print '[SucSide] side waypoint A'
else:
side_waypoint1 = side_waypoint1b
print '[SucSide] side waypoint B'
#side_waypoint2=sidepos-horizontal_dim+side_offset
side_waypoint2 = binRight + horizontal_dim + horz_offset
if side_waypoint2 > side_waypoint1:
if side_waypoint2 > side_waypoint1a:
print '[SucSide] Not enought gap'
return (False, False)
else:
side_waypoint1 = side_waypoint2
targetPositionList = [
[binMouth[0] - .15, side_waypoint1, target_height + up_down_adjust],
[objPosition[0], side_waypoint1, target_height + up_down_adjust],
[objPosition[0], side_waypoint2, target_height + up_down_adjust],
[objPosition[0], side_waypoint2, target_height]
]
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 '[SucSide] move to COM in y successful'
print '[SucSide] tcp at:', targetPosition
plan.visualize(hand_param=hand_gap)
plans.append(plan)
#if isExecute:
# pauseFunc(withPause)
# plan.execute()
else:
print '[SucSide] move to COM in y fail'
return (False, False)
#print plan.q_traj
qf = plan.q_traj[-1]
print '[SucSide] qf:', qf
#set robot speed
setSpeedByName(speedName='faster')
for numOfPlan in range(0, len(plans)):
if numOfPlan >= len(plans) - 2:
setSpeedByName(speedName='fast')
if isExecute:
plans[numOfPlan].visualize(hand_param=hand_gap)
pauseFunc(withPause)
plans[numOfPlan].execute()
suction.start()
final_hand_gap = 110
gripper.set_force(12)
gripper.grasp(move_pos=final_hand_gap)
gripper.close()
hand_gap = final_hand_gap
print '[SucSide] hand_gap:', hand_gap
continue_suction = suction_items(objId)
if continue_suction:
print '[SucSide] object is of type that suction side will try to remove from bin'
else:
print '[SucSide] object is to big to remove from bin'
## 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()
if not continue_suction:
suction.stop()
rospy.sleep(3)
print '[SucSide] Is suction in contact still? Lets see:'
print '[SucSide] suction.check(): ', suction.check()
print '[SucSide] suction.check(): ', suction.check()
if suction.check():
#set robot speed
print '[SucSide] got item. continuing. suction'
setSpeedByName(speedName='fast')
return (True, True)
else:
print '[SucSide] did not get item. Stopping suction'
suction.stop()
return (True, False)
def main(argv=None):
if argv is None:
argv = sys.argv
rospy.init_node('calib', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(1)
calibtag = 'link6tag1'
if len(argv) == 2:
calibtag = argv[1]
#filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/camera_extrinsic_calib_data/camera_extrinsic_calib_data.json'
if calibtag == 'link2tag':
tag_link_transform = [0, 0.7, -0.2145-0.005, -math.pi, 0, 0]
link_frame_id = 'link_2'
elif calibtag == 'realsense1':
print 'move to q =', (np.array([ 0.0, -23.02, 34.73, 154.93, -25.15, 0.0]) / 180 * math.pi).tolist()
r = 0.1923/2
tag_link_transform = [0.4033-0.230 -r, -1.121/2 + 0.044 + r, 0.005, 0, 0, 0]
link_frame_id = 'base_link'
elif calibtag == 'link6tag1':
#print 'move to q =', (np.array([9.93,39.37,-30.63,60.64,-12.12,-61.79]) / 180 * math.pi).tolist()
tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi/2]
link_frame_id = 'link_6'
elif calibtag == 'link6tag2':
tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, 0]
link_frame_id = 'link_6'
elif calibtag == 'link6tag3':
tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, -math.pi/2]
link_frame_id = 'link_6'
elif calibtag == 'link6tag4':
tag_link_transform = [-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi]
link_frame_id = 'link_6'
# visualize it
pubFrame(br, pose=tag_link_transform, frame_id='tag', parent_frame_id=link_frame_id, npub=10)
tag_map_transform = poseTransform(tag_link_transform, link_frame_id, 'map', listener)
apriltag_topic = '/pr_apriltags/detections'
print 'Wait for apriltag detection'
tag_camera_transforms = []
for i in xrange(5):
while True:
tagdetect = ROS_Wait_For_Msg(apriltag_topic, AprilTagDetections).getmsg()
if len(tagdetect.detections) == 1:
tag_camera_transform = pose2list(tagdetect.detections[0].pose)
tag_camera_transforms.append(tag_camera_transform)
break
tag_camera_transform = np.average(np.array(tag_camera_transforms), axis = 0)
tag_camera_tfm_mat = matrix_from_xyzquat(tag_camera_transform)
tag_map_tfm_mat = matrix_from_xyzquat(tag_map_transform)
camera_map_tfm_mat = np.dot(tag_map_tfm_mat, np.linalg.inv(tag_camera_tfm_mat))
camera_map_pose = tfm.translation_from_matrix(camera_map_tfm_mat).tolist() + tfm.quaternion_from_matrix(camera_map_tfm_mat).tolist()
link6tag = ['link6tag1', 'link6tag2', 'link6tag3', 'link6tag4']
if calibtag in link6tag:
print camera_map_pose
pubFrame(br, pose=camera_map_pose, frame_id='new_kinect_pose', parent_frame_id='map', npub=10)
elif calibtag == 'realsense1':
realsense_link5_pose = poseTransform(camera_map_pose, link_frame_id, 'link_5', listener)
pubFrame(br, pose=realsense_link5_pose, frame_id='new_realsense_pose', parent_frame_id='link_5', npub=10)
print realsense_link5_pose
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 main(argv=None):
if argv is None:
argv = sys.argv
rospy.init_node('calib', anonymous=True)
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(1)
calibtag = 'link6tag1'
if len(argv) == 2:
calibtag = argv[1]
# kinect calib joints
# j1: 9.93
# j2: 39.46
# j3: -32.74
# j4: 69.54
# j5: -11.26
# j6: -70.88
#link6tag1
#link6tag3
#link6tag1
#link6tag3
# j1: -16.98
# j2: 65.55
# j3: -13.48
# j4: -21.15
# j5: -51.26
# j6: 10.63
#link6tag1
#link6tag3
#filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/camera_extrinsic_calib_data/camera_extrinsic_calib_data.json'
if calibtag == 'link2tag':
tag_link_transform = [0, 0.7, -0.2145 - 0.005, -math.pi, 0, 0]
link_frame_id = 'link_2'
elif calibtag == 'linkbasetag':
print 'move to q =', (
np.array([2.35, -67.73, 62.39, -154.9, -30.11, 11]) / 180 *
math.pi).tolist()
print 'move to q =', (
np.array([2.35, -77.73, 72.39, -154.9, -20.11, 11]) / 180 *
math.pi).tolist()
tag_link_transform = [0.5291, 0.4428, 0.005, math.pi, 0, 0]
link_frame_id = 'base_link'
elif calibtag == 'link6tag1':
#print 'move to q =', (np.array([9.93,39.37,-30.63,60.64,-12.12,-61.79]) / 180 * math.pi).tolist()
tag_link_transform = [
-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0,
math.pi / 2
]
link_frame_id = 'link_6'
elif calibtag == 'link6tag2':
tag_link_transform = [
-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, 0
]
link_frame_id = 'link_6'
elif calibtag == 'link6tag3':
tag_link_transform = [
-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0,
-math.pi / 2
]
link_frame_id = 'link_6'
elif calibtag == 'link6tag4':
tag_link_transform = [
-(2.54 + 4 + 1 + 19.23 / 2) / 100.0, 0, 0.01, math.pi, 0, math.pi
]
link_frame_id = 'link_6'
# visualize it
pubFrame(br,
pose=tag_link_transform,
frame_id='tag',
parent_frame_id=link_frame_id,
npub=10)
tag_map_transform = poseTransform(tag_link_transform, link_frame_id, 'map',
listener)
apriltag_topic = 'tag_detections'
while True:
tagdetect = ROS_Wait_For_Msg(apriltag_topic,
AprilTagDetectionArray).getmsg()
if len(tagdetect.detections) == 1:
tag_kinect_transform = pose2list(tagdetect.detections[0].pose.pose)
break
tag_kinect_tfm_mat = matrix_from_xyzquat(
translate=tag_kinect_transform[0:3],
quaternion=tag_kinect_transform[3:7])
tag_map_tfm_mat = matrix_from_xyzquat(translate=tag_map_transform[0:3],
quaternion=tag_map_transform[3:7])
kinect_map_tfm_mat = np.dot(tag_map_tfm_mat,
np.linalg.inv(tag_kinect_tfm_mat))
kinect_map_pose = tfm.translation_from_matrix(kinect_map_tfm_mat).tolist(
) + tfm.quaternion_from_matrix(kinect_map_tfm_mat).tolist()
link6tag = ['link6tag1', 'link6tag2', 'link6tag3', 'link6tag4']
if calibtag in link6tag:
print kinect_map_pose
pubFrame(br,
pose=kinect_map_pose,
frame_id='new_kinect_pose',
parent_frame_id='map',
npub=10)
else:
realsense_link5_pose = poseTransform(kinect_map_pose, link_frame_id,
'link_5', listener)
pubFrame(br,
pose=realsense_link5_pose,
frame_id='new_realsense_pose',
parent_frame_id='link_5',
npub=10)
print realsense_link5_pose