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