def recover(slot_pos_obj, reset):
global z, zup
#import pdb; pdb.set_trace()
z_recover = 0.011 + z # the height for recovery
z_offset = z_recover
slot_pos_obj = slot_pos_obj + [0]
_center_world = copy.deepcopy(center_world)
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransformList(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world, ori)
# move down a bit in fast speed
pos_recover_probe_world = coordinateFrameTransformList(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = z_offset + 0.02
setCart(pos_recover_probe_world, ori)
# move down to the slot
# speed down
setAcc(acc=globalhighacc, deacc=globalacc)
setSpeed(tcp=60, ori=1000)
pos_recover_probe_world[2] = z_offset
setCart(pos_recover_probe_world, ori)
#pause()
# move to the world center
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = z_offset
setCart(pos_recover_probe_target_world, ori)
# speed up
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=1000)
# move up
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = zup + 0.06 #
setCart(pos_recover_probe_target_world, ori)
def recover(slot_pos_obj, reset):
z_recover = 0.011 + z # the height for recovery
z_offset = z_recover
slot_pos_obj = slot_pos_obj + [0]
_center_world = copy.deepcopy(center_world)
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransformList(
slot_pos_obj, obj_frame_id, global_frame_id, lr)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world)
# move down a bit in fast speed
pos_recover_probe_world = coordinateFrameTransformList(
slot_pos_obj, obj_frame_id, global_frame_id, lr)
pos_recover_probe_world[2] = z_offset + 0.02
setCart(pos_recover_probe_world)
# move down to the slot
# speed down
setSpeed(tcp=global_slow_vel)
pos_recover_probe_world[2] = z_offset
setCart(pos_recover_probe_world)
#pause()
# move to the world center
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = z_offset
setCart(pos_recover_probe_target_world)
# speed up
setSpeed(tcp=global_high_vel)
# move up
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = zup + 0.06 #
setCart(pos_recover_probe_target_world)
def main():
rospy.init_node('apriltag_cone')
rospy.Subscriber('/pose_est', String, rosApriltagCallback)
lr = tf.TransformListener()
rospy.sleep(1)
half = 0.016
top = coordinateFrameTransformList([0, 0, 0], 'observer_rgb_optical_frame', 'map', lr)
marker_id = 62
while not rospy.is_shutdown():
rgba = [1,1,0,0.8]
if notagcount == 0:
rgba = [1,1,0,0.8]
elif notagcount > 5:
rgba = [0,1,1,0]
X1 = coordinateFrameTransformList([half, half, 0.01], 'apriltag0_vicon', 'map', lr)
X2 = coordinateFrameTransformList([half, -half, 0.01], 'apriltag0_vicon', 'map', lr)
X3 = coordinateFrameTransformList([-half, -half, 0.01], 'apriltag0_vicon', 'map', lr)
X4 = coordinateFrameTransformList([-half, half, 0.01], 'apriltag0_vicon', 'map', lr)
vis_pub.publish(createTriangleListMarker(marker_id, [X1, X4, X3, X2], rgba = rgba, frame_id = '/map', o=Point(*top[0:3])))
rospy.sleep(0.01)
def run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt):
# hack to restart the script to prevent ros network issues.
global pub
limit = 100
cnt = 0
topics = ['-a']
# enumerate the possible trajectories
for cnt_acc, acc in enumerate(accelerations):
# enumerate the side we want to push
for i in range(nside):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
pos = np.array(shape[i]) *(1-s) + np.array(shape[(i+1) % len(shape)]) *(s)
pos = np.append(pos, [0])
tangent = np.array(shape[(i+1) % len(shape)]) - np.array(shape[i])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
elif shape_type == 'ellip':
(a,b) = shape[0][0], shape[0][1]
pos = [shape[0][0] * np.cos(s*2*np.pi), shape[0][1] * np.sin(s*2*np.pi), 0]
normal = [np.cos(s*2*np.pi)/a, np.sin(s*2*np.pi)/b, 0]
normal = normal / norm(normal) # normalize it
elif shape_type == 'polyapprox':
pos, normal = polyapprox(shape, s)
# enumerate the direction in which we want to push
for t in angles:
for rep in xrange(nrep):
if nrep > 1:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
else:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t)
bagfilepath = dir_save_bagfile+bagfilename
# if exists then skip it
if skip_when_exists and os.path.isfile(bagfilepath):
#print bagfilepath, 'exits', 'skip'
continue
# find the probe pos in contact in object frame
pos_probe_contact_object = pos + normal * probe_radius
# find the start point
direc = np.dot(tfm.euler_matrix(0,0,t) , normal.tolist() + [1])[0:3] # in the direction of moving out
pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact
if shape_type == 'polyapprox' and polyapprox_check_collision(shape, pos_start_probe_object, probe_radius):
print bagfilename, 'will be in collision', 'skip'
continue
# find the end point
pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact
# zero force torque sensor
rospy.sleep(0.1)
#setZero()
#wait_for_ft_calib()
# transform start and end to world frame
if hasRobot:
pos_start_probe_world = coordinateFrameTransformList(pos_start_probe_object, obj_frame_id, global_frame_id, listener)
pos_end_probe_world = coordinateFrameTransformList(pos_end_probe_object, obj_frame_id, global_frame_id, listener)
pos_contact_probe_world = coordinateFrameTransformList(pos_probe_contact_object, obj_frame_id, global_frame_id, listener)
pos_center_obj_world = coordinateFrameTransformList([0,0,0], obj_frame_id, global_frame_id, listener)
else:
pos_start_probe_world = pos_start_probe_object + center_world
pos_end_probe_world = pos_end_probe_object + center_world
pos_contact_probe_world = pos_probe_contact_object + center_world
pos_center_obj_world = center_world
# start bag recording
# move to startPos
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=global_highvel)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos,ori)
setAcc(acc=globalhighacc, deacc=globalacc)
setSpeed(tcp=global_highvel, ori=1000)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos,ori)
setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
pub.publish('move_to_mid_pos')
setCart(mid_pos,ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if acc == 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=speeds[cnt_acc], ori=1000)
else: # there is acceleration
setAcc(acc=acc, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
pub.publish('start_pushing')
setCart(end_pos,ori)
pub.publish('end_pushing')
# end bag recording
helper.terminate_ros_node("/record")
setSpeed(tcp=global_highvel, ori=1000)
setAcc(acc=globalhighacc, deacc=globalhighacc)
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos,ori)
distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world))
# recover
recover(obj_slot, distance_obj_center > allowed_distance)
#pause()
cnt += 1
if cnt > limit: return