def get_corresponding_data(strm1, strm2): """ Returns two lists of same-length where the corresponding entries occur at the same time-stamp. STRM1, STRM2 are the two streams for which the corresponding entries are to be found. It also returns the indices at which data from both the streams is found. It assumes that the two streams start at the same time-scale. """ dat1, dat2, inds = [],[],[] idx = 0 while True: try: d1 = strm1.next() d2 = strm2.next() if d1!=None and d2!= None: dat1.append(d1) dat2.append(d2) inds.append(idx) idx += 1 except: break strm1.reset() strm2.reset() N = len(dat1) print colorize("Found %s corresponding data points." % colorize(str(N), "red", True), "blue", True) return (inds, dat1, dat2)
def grab_rope(self, lr, seg_samples=5): print colorize("TESTING FOR GRAB...", "magenta", True) nodes, ctl_pts = self.rope.GetNodes(), self.rope.GetControlPoints() graspable_inds = [] for i in xrange(len(ctl_pts)-1): pts = math_utils.interp2d(np.linspace(0,1,seg_samples), [0,1], np.r_[np.reshape(ctl_pts[i], (1,3)), np.reshape(ctl_pts[i+1], (1,3))]) if np.any([in_grasp_region(self.robot, lr, pt) for pt in pts]): graspable_inds.append(i) print colorize('graspable inds for %s: %s' % (lr, str(graspable_inds)), "magenta", True) if len(graspable_inds) == 0: return False robot_link = self.robot.GetLink("%s_gripper_l_finger_tip_link"%lr) rope_links = self.rope.GetKinBody().GetLinks() for i_node in graspable_inds: for i_cnt in range(max(0, i_node-1), min(len(nodes), i_node+2)): cnt = self.bt_env.AddConstraint({ "type": "generic6dof", "params": { "link_a": robot_link, "link_b": rope_links[i_cnt], "frame_in_a": np.linalg.inv(robot_link.GetTransform()).dot(rope_links[i_cnt].GetTransform()), "frame_in_b": np.eye(4), "use_linear_reference_frame_a": False, "stop_erp": .8, "stop_cfm": .1, "disable_collision_between_linked_bodies": True, } }) self.constraints[lr].append(cnt) return True
def publish_transform_markers(grabber, marker, T, from_frame, to_frame, rate=100): from visualization_msgs.msg import Marker from sensor_msgs.msg import PointCloud2 print colorize("Transform : ", "yellow", True) print T marker_frame = "ar_frame" tf_pub = tf.TransformBroadcaster() pc_pub = rospy.Publisher("camera_points", PointCloud2) trans = T[0:3,3] rot = tf.transformations.quaternion_from_matrix(T) sleeper = rospy.Rate(10) while True: try: r, d = grabber.getRGBD() ar_tfm = get_ar_transform_id (d, r, marker) pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, from_frame) pc_pub.publish(pc) tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame) try: trans_m, rot_m = conversions.hmat_to_trans_rot(ar_tfm) tf_pub.sendTransform(trans_m, rot_m, rospy.Time.now(), marker_frame, from_frame) except: pass sleeper.sleep() except KeyboardInterrupt: break
def get_transforms(arm, hydra, n_tfm , n_avg): """ Returns a tuple of two list of N_TFM transforms, each the average of N_AVG transforms corresponding to the left/ right arm of the pr2 and the hydra paddle of the HYDRA ('right' or 'left') side. """ pr2_frame = 'base_footprint' assert arm=='right' or 'left' arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm] hydra_frame = 'hydra_base' assert hydra=='right' or hydra=='left' paddle_frame = 'hydra_%s'%hydra tf_sub = tf.TransformListener() I_0 = np.eye(4) I_0[3,3] = 0 arm_tfms = [] hydra_tfms = [] for i in xrange(n_tfm+1): raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm), "red", True)) ## transforms which need to be averaged. p_ts = np.empty((0, 3)) p_qs = np.empty((0,4)) h_ts = np.empty((0, 3)) h_qs = np.empty((0,4)) sleeper = rospy.Rate(30) for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True) ptrans, prot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0)) htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0)) p_ts = np.r_[p_ts, np.array(ptrans, ndmin=2)] h_ts = np.r_[h_ts, np.array(htrans, ndmin=2)] p_qs = np.r_[p_qs, np.array(prot, ndmin=2)] h_qs = np.r_[h_qs, np.array(hrot, ndmin=2)] sleeper.sleep() ptrans_avg = np.sum(p_ts, axis=0) / n_avg prot_avg = avg_quaternions(p_qs) htrans_avg = np.sum(h_ts, axis=0) / n_avg hrot_avg = avg_quaternions(h_qs) a_tfm = conversions.trans_rot_to_hmat(ptrans_avg,prot_avg) h_tfm = conversions.trans_rot_to_hmat(htrans_avg,hrot_avg) arm_tfms.append(a_tfm) hydra_tfms.append(h_tfm) return (arm_tfms, hydra_tfms)
def __check_time__(self, t): if self.t_filt == None: print colorize('[Filter ERROR:] Filter not initialized.', 'red', True) return False if self.t_filt >= t: print colorize('[Filter ERROR:] Observation behind the filter estimate in time. Ignoring update.', 'blue', True) return False return True
def publish_tf(T, from_frame, to_frame): print colorize("Transform : ", "yellow", True) print T tf_pub = tf.TransformBroadcaster() trans = T[0:3,3] rot = tf.transformations.quaternion_from_matrix(T) sleeper = rospy.Rate(100) while True: tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame) sleeper.sleep()
def generate_test_cmdline_params(demo_type, generate_h5s=False, sizes = [50, 25, 4], use_ik=False): """ saves ~6000 sets of command line arguments. """ rope_lengths = get_rope_lengths(demo_type) perturb_fname = init_perturbation_map[demo_type] + '_perturb.h5' perturb_fname = osp.join(perturbations_dir, perturb_fname) perturb_file = h5py.File(perturb_fname, "r") subsets = split_pertubations_into_two(perturb_fname, sizes) if generate_h5s: if use_ik: generate_testing_h5_file_ik(demo_type, subsets[0], subsets[1]) else: generate_testing_h5_files(demo_type, subsets[0], subsets[1], rope_lengths) cmdline_params = [] cmdline_dir = testing_commands_dir if not osp.exists(cmdline_dir): os.mkdir(cmdline_dir) num_demos = len(rope_lengths) * np.sort(np.array(subsets[0].keys()).astype(int)) num_perts = 5 print num_perts for idx_ndemos, ndemos in enumerate(num_demos): ## x3 for demo_set in [0,1]: ## x2 for init_set in [0,1]: ## x2 init_subset = subsets[init_set] for init_demo_idx in init_subset[np.max(init_subset.keys())]: ## x50 init_config_data = perturb_file[perturb_file.keys()[init_demo_idx]] init_demo_name = perturb_file.keys()[init_demo_idx] for init_perturb_name in init_config_data.keys()[0:num_perts]: ## x10 demo_data_h5_prefix = "size%d_set%d"%(ndemos, demo_set+1) init_state_h5 = init_perturbation_map[demo_type] if len(rope_lengths) > 1: rope_scaling_factor = sample_rope_scaling(rope_lengths) else: rope_scaling_factor = 1.0 results_fname = osp.join(demo_type, "%d_demos"%ndemos, "initset%d_demoset%d"%(init_set+1, demo_set+1), "perturb_%s_%s.cp"%(init_demo_name, init_perturb_name)) cmdline_params.append([ndemos, demo_type, demo_data_h5_prefix, init_state_h5, str(init_demo_name), str(init_perturb_name), rope_scaling_factor, str(results_fname)]) cmdline_file = osp.join(cmdline_dir, "%s_cmds.cp"%demo_type) cp.dump(cmdline_params, open(cmdline_file, 'w')) print colorize("wrote : %s"%cmdline_file, "yellow", True)
def fit_and_plot_tps(plot=False): T_cam_gt, T_hy = pickle.load(open('grp.dat', 'r')) T_cam_gt = T_cam_gt[100:len(T_cam_gt)-100] T_hy = T_hy[100:len(T_hy)-100] BLOCK_SIZE = 10 N = len(T_hy) train, tst = gen_indices(N, BLOCK_SIZE) N_train = len(train) N_test = len(tst) T_hy_train = [T_hy[i] for i in train] T_hy_test = [T_hy[i] for i in tst] T_cam_gt_train = [T_cam_gt[i] for i in train] T_cam_gt_test = [T_cam_gt[i] for i in tst] x_train = np.empty((N_train, 3)) x_test = np.empty((N_test, 3)) x_gt_train = np.empty((N_train, 3)) x_gt_test = np.empty((N_test, 3)) for i in xrange(N_train): x_train[i,:] = T_hy_train[i][0:3,3] x_gt_train[i,:] = T_cam_gt_train[i][0:3,3] for i in xrange(N_test): x_test[i,:] = T_hy_test[i][0:3,3] x_gt_test[i,:] = T_cam_gt_test[i][0:3,3] # fit tps: x_gt_train = x_gt_train[100:-100] x_train = x_train[100:-100] print colorize("Fitting TPS on %d data-points."%len(x_train), "red", True) f_tps = fit_tps(x_gt_train, x_train, plot=plot) ## fit tps on test data: x_pred = f_tps(x_test) ## plot: if plot: for i in xrange(3): plt.subplot(3,1,i+1) plt.plot(x_test[:,i],label='hydra') plt.plot(x_gt_test[:,i],label='cam') plt.plot(x_pred[:,i],label='pred') plt.legend() plt.show()
def process_request(self, req): if req['type'] == 'custom': try: f = self.plotting_funcs[req['func']] except KeyError: print colorize("No custom plotting function with name : %s. Ignoring plot request."%req['func'], "red") elif req['type'] == 'mlab': try: f = getattr(mlab, req['func']) except KeyError: print colorize("No mlab plotting function with name : %s. Ignoring plot request."%req['func'], "red") args, kwargs = req['data'] f(*args, **kwargs)
def get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs): """ Return the first state (mean, covar) to initialize the kalman filter with. Assumes that the time-stamps start at a common zero (are on the same time-scale). Returns a state b/w t=[0, dt] Gives priority to AR-markers. If no ar-markers are found in [0,dt], it returns hydra's estimate but with larger covariance. """ ar1 = [c1_tfs[i] for i in xrange(len(c1_ts)) if c1_ts[i] <= dt] ar2 = [c2_tfs[i] for i in xrange(len(c2_ts)) if c2_ts[i] <= dt] hy = [hy_tfs[i] for i in xrange(len(hy_ts)) if hy_ts[i] <= dt] if ar1 != [] or ar2 != []: ar1.extend(ar2) x0 = state_from_tfms_no_velocity([avg_transform(ar1)]) I3 = np.eye(3) S0 = scl.block_diag(1e-3*I3, 1e-2*I3, 1e-3*I3, 1e-3*I3) else: assert len(hy)!=0, colorize("No transforms found for KF initialization. Aborting.", "red", True) x0 = state_from_tfms_no_velocity([avg_transform(hy)]) I3 = np.eye(3) S0 = scl.block_diag(1e-1*I3, 1e-1*I3, 1e-2*I3, 1e-2*I3) return (x0, S0)
def publish_transform(T, from_frame, to_frame, rate=100): print colorize("Transform : ", "yellow", True) print T tf_pub = tf.TransformBroadcaster() trans, rot = conversions.hmat_to_trans_rot(T) sleeper = rospy.Rate(10) while True: try: tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame) sleeper.sleep() except KeyboardInterrupt: break
def get_transforms(arm, hydra, n_tfm , n_avg): """ Returns a tuple of two list of N_TFM transforms, each the average of N_AVG transforms corresponding to the left/ right arm of the pr2 and the hydra paddle of the HYDRA ('right' or 'left') side. return arm_tfms, hydra_tfms """ pr2_frame = 'base_footprint' assert arm == 'right' or 'left' arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm] hydra_frame = 'hydra_base' assert hydra == 'right' or hydra == 'left' paddle_frame = 'hydra_%s' % hydra tf_sub = tf.TransformListener() I_0 = np.eye(4) I_0[3, 3] = 0 gripper_tfms = [] hydra_tfms = [] for i in xrange(n_tfm): raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm - 1), "red", True)) # # transforms which need to be averaged. gtfms = [] htfms = [] sleeper = rospy.Rate(30) for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True) gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0)) gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot)) htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0)) htfms.append(conversions.trans_rot_to_hmat(htrans, hrot)) sleeper.sleep() gripper_tfms.append(avg_transform(gtfms)) hydra_tfms.append(avg_transform(htfms)) return (gripper_tfms, hydra_tfms)
def get_tool_model(n_tfm, n_avg): listener = tf.TransformListener() gripper_frame = 'l_gripper_tool_frame' base_frame = 'base_link' model_frame = '' depth_frame = 'camera_depth_optical_frame' head_frame = 'head_plate_frame' model_tfms = [] tool_tfms = [] for i in xrange(n_tfm): raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i + 1, n_tfm), "red", True)) tool_trans_list = np.empty((0, 3)) tool_quat_list = np.empty((0, 4)) marker_trans_list = np.empty((0, 3)) marker_quat_list = np.empty((0, 4)) head_trans_list = np.empty((0, 3)) head_quat_list = np.empty((0, 4)) time.sleep(3) for j in xrange(n_avg): tool_trans, tool_quat, model_trans, model_quat, head_trans, head_quat = None, None, None, None, None, None while tool_trans == None: #model_trans, model_quat = listener.lookupTransform(base_frame, model_frame, rospy.Time(0)) tool_trans, tool_quat = listener.lookupTransform(base_frame, gripper_frame, rospy.Time(0)) head_trans, head_quat = listener.lookupTransform(base_frame, head_frame, rospy.Time(0)) tool_trans_list = np.r_[tool_trans_list, np.array(tool_trans, ndmin=2)] tool_quat_list = np.r_[tool_quat_list, np.array(tool_quat, ndmin=2)] #model_trans_list = np.r_[model_trans_list, np.array(model_trans, ndmin=2)] #model_quat_list = np.r_[model_quat_list, np.array(model_quat, ndmin=2)] head_trans_list = np.r_[head_trans_list, np.array(head_trans, ndmin=2)] head_quat_list = np.r_[head_quat_list, np.array(head_quat, ndmin=2)] tool_trans_avg = np.sum(tool_trans_list, axis=0) / n_avg tool_quat_avg = avg_quaternions(tool_quat_list) #model_trans_avg = np.sum(model_trans_list, axis=0) / n_avg #model_quat_avg = avg_quaternions(model_quart_list) head_trans_avg = np.sum(head_trans_list, axis=0) / n_avg head_quat_avg = avg_quaternions(head_quat_list) tool_tfm = conversions.trans_rot_to_hmat(tool_trans_avg, tool_quat_avg) head_tfm = conversions.trans_rot_to_hmat(head_trans_avg, head_quat_avg) #depth_to_model_tfm = conversions.trans_rot_to_hmat(model_trans_avg, model_quat_avg) depth_tfm = head_tfm.dot(T_h_k) #model_tfm = depth_tfm.dot(depth_to_model_tfm) tool_tfms.append(tool_tfm) #model_tfms.append(model_tfm) #return tool_tfms, model_tfms return tool_tfms
def call_on_cloud(cmd_params, core_type, num_batches, start_batch_num, end_batch_num): ntests = len(cmd_params) batch_size = int(math.ceil(ntests/(num_batches+0.0))) batch_edges = batch_size*np.array(xrange(num_batches))[start_batch_num : end_batch_num] print batch_edges for i in xrange(len(batch_edges)): if i==len(batch_edges)-1: cmds = cmd_params[batch_edges[i]:] else: cmds = cmd_params[batch_edges[i]:min(batch_edges[i+1], len(cmd_params))] print colorize("calling on cloud..", "yellow", True) try: jids = cloud.map(run_sim_test, cmds, _vol='rss_dat', _env='RSS3', _type=core_type) res = cloud.result(jids) print colorize("got results for batch %d/%d "%(i, len(batch_edges)), "green", True) save_results(res) except Exception as e: print "Found exception %s. Not saving data for this demo."%e
def __init__(self): self.finished = False devices = glob.glob("/dev/ttyACM*") if len(devices) ==0: raise RuntimeError(colorize( "Arduino not connected.", 'red', True)) elif len(devices) >1: print colorize("Found more than one matching devices. Enter the number of the one which is arduino:", 'blue', True) for n, d in enumerate(devices): print "\t%d. %s"%(n,d) while True: try: sel = int(raw_input()) if 0<= sel < len(devices): break except: print "\tPlease enter a number in the range [0, %d]" % len(devices) pass arduino = devices[sel] else: arduino = devices[0] try: self.ser = serial.Serial(arduino, 9600, timeout=1) except serial.SerialException as e: raise RuntimeError(colorize("Error opening serial port '{}': {}".format(arduino, e), "red", True)) self.poll_thread = Thread(target=self.poll_arduino) self.poll_thread.daemon = True self.poll_thread.start() time.sleep(0.1) print colorize("Arduino all set.", "green", True)
def plot_warping(f, src, target, fine=True, draw_plinks=True): """ function to plot the warping as defined by the function f. src : nx3 array target : nx3 array fine : if fine grid else coarse grid. """ print colorize("Plotting grid ...", "blue", True) mean = np.mean(src, axis=0) mins = np.min(src, axis=0) maxes = np.max(src, axis=0) grid_lines = [] if fine: grid_lines = gen_grid2(f, mins=mins, maxes=maxes, xres=0.005, yres=0.005, zres=0.002) else: grid_lines = gen_grid(f, mins=mins, maxes=maxes) plotter_requests = [] plotter_requests.append(gen_mlab_request(mlab.clf)) plotter_requests.append(gen_custom_request("lines", lines=grid_lines, color=(0, 0.5, 0.3))) warped = f(src) plotter_requests.append( gen_mlab_request(mlab.points3d, src[:, 0], src[:, 1], src[:, 2], color=(1, 0, 0), scale_factor=0.001) ) plotter_requests.append( gen_mlab_request(mlab.points3d, target[:, 0], target[:, 1], target[:, 2], color=(0, 0, 1), scale_factor=0.001) ) plotter_requests.append( gen_mlab_request(mlab.points3d, warped[:, 0], warped[:, 1], warped[:, 2], color=(0, 1, 0), scale_factor=0.001) ) if draw_plinks: plinks = [np.c_[ps, pw].T for ps, pw in zip(src, warped)] plotter_requests.append(gen_custom_request("lines", lines=plinks, color=(0.5, 0, 0), line_width=2, opacity=1)) return plotter_requests
def process_observation (self, n_avg=5): self.iterations += 1 raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True)) avg_tfms = {} for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True) tfms = {} tfms.update(gmt.get_ar_markers_from_cameras(self.cameras, parent_frame=self.parent_frame, markers=self.ar_markers)) tfms.update(gmt.get_hydra_transforms(parent_frame=parent_frame, hydras = self.hydras)) pot_angle = gmt.get_pot_angle() for marker in tfms: if marker not in avg_tfms: avg_tfms[marker] = [] avg_tfms[marker].append(tfms[marker]) for marker in avg_tfms: avg_tfms[marker] = utils.avg_transform(avg_tfms[marker]) update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
def call_pnp_app(demo_type): demo_testing_dir = osp.join(testing_results_dir, demo_type) sub = ['12_demos', '75_demos', '150_demos'] subsub = ['initset1_demoset1', 'initset1_demoset2', 'initset2_demoset1', 'initset2_demoset2'] results = {} for s in sub: res_ss = {} for ss in subsub: snapshot_path = osp.join(demo_testing_dir, s, ss, 'snapshots') save_path = '/tmp/%d.cp'%np.random.randint(10000000000) print colorize('doing : %s/%s'%(s,ss), "green", True) subprocess.call('python /home/ankush/sandbox444/human_demos/hd_evaluate/pnpApp/pnpApp.py --snapshot_path=%s --save_path=%s'%(snapshot_path, save_path), shell=True) stats = cp.load(open(save_path,'r')) res_ss[ss] = stats results[s] = res_ss stats_file = osp.join(demo_testing_dir, 'stats.cp') with open(stats_file,'w') as f: cp.dump(results, f) print colorize("all done", "red", True)
def split_pertubations_into_two(perturb_fname, sizes = [50, 25, 4]): perturb_dat = h5py.File(perturb_fname, 'r') num_perturbs = len(perturb_dat.keys()) assert num_perturbs >= 90, colorize("not enough demos in the perturbation file : %s"%perturb_fname) subset1, subset2 = {}, {} for i in xrange(len(sizes)): if i==0: random_indices = range(num_perturbs) np.random.shuffle(random_indices) subset1[sizes[i]] = random_indices[:sizes[i]] subset2[sizes[i]] = random_indices[sizes[i]:] else: subset1[sizes[i]] = [subset1[sizes[i-1]][x] for x in np.random.choice(range(len(subset1[sizes[i-1]])), sizes[i], replace=False)] subset2[sizes[i]] = [subset2[sizes[i-1]][x] for x in np.random.choice(range(len(subset2[sizes[i-1]])), sizes[i], replace=False)] perturb_dat.close() return subset1, subset2
def generate_testing_h5_file_ik(demo_type, subset1, subset2): """ demo_type : the type of the knot : {overhand, square-knot, ...} subset1, subset2 : the splitting indices as returned by split_perturbations_into_two above """ def copy_to_subset(parent_h5, child_h5, subset): for idx in subset: if idx >= len(parent_h5.keys()): continue pkey = parent_h5.keys()[idx] parent_h5.copy(pkey, child_h5) rope_lengths = [140] demo_h5_fnames = {l : osp.join(demo_files_dir, "%s"%(demo_type), "%s.h5"%(demo_type)) for l in [140]} demo_h5_files = {} for h5_fname in demo_h5_fnames.values(): if not osp.exists(h5_fname): print colorize(" %s : does not exist. Exiting..."%h5_fname, "red", True) sys.exit(-1) for l in rope_lengths: demo_h5_files[l] = h5py.File(demo_h5_fnames[l], 'r') out_dir = osp.join(data_dir, "testing_h5s", demo_type) if not osp.exists(out_dir): os.mkdir(out_dir) num_lengths = len(rope_lengths) subset_sizes = np.array([np.sort(np.array(subset1.keys()))]) cumm_subset_sizes = num_lengths * subset_sizes.astype(int) for i,cumm_size in enumerate(cumm_subset_sizes): for l in rope_lengths: subset1_h5_name, subset2_h5_name = [osp.join(out_dir, "size%d_set%d.h5"%(cumm_size, x)) for x in [1,2]] subset1_h5_file = h5py.File(subset1_h5_name, 'w') subset2_h5_file = h5py.File(subset2_h5_name, 'w') copy_to_subset(demo_h5_files[l], subset1_h5_file, subset1[np.sort(subset1.keys())[i]]) copy_to_subset(demo_h5_files[l], subset2_h5_file, subset2[np.sort(subset2.keys())[i]]) subset1_h5_file.close() subset2_h5_file.close() print colorize("saved : %s"%subset1_h5_name, "green", True) print colorize("saved : %s"%subset2_h5_name, "green", True) for h5file in demo_h5_files.values(): h5file.close()
def test_all(stop=False): nPass,nFail = 0,0 for (name,func) in TEST_FUNCS.items(): print colorize("function: %s"%name,"green") try: t_start = time() func() t_elapsed = time() - t_start print colorize("PASSED (%.3f sec)"%t_elapsed,"blue") nPass += 1 except Exception: traceback.print_exc(file=sys.stdout) if stop: raise print colorize("FAILED","red") nFail += 1 print "%i passed, %i failed"%(nPass,nFail)
def create_graph_from_observations(parent_frame, calib_info, min_obs=5, n_avg=5, freq=None): """ Runs a loop till graph has enough data and user is happy with data. Or run with frequency specified until enough data is gathered. @parent_frame -- frame to get observations in. @num_markers -- total_number of markers. @calib_info -- dict with information on what groups and markers to search for. also gives information on which is the master group and how the angle affects group. @min_obs -- minimum number of observations for transform required after finding it once. @n_avg -- number of times to average transform per observation. """ global tf_listener if rospy.get_name() == "/unnamed": rospy.init_node("gripper_marker_calibration") tf_listener = tf.TransformListener() tf_listener.clear() masterGraph = nx.DiGraph() ar_markers = [] hydras = [] ps_markers = [] # Setup the groups for group in calib_info: masterGraph.add_node(group) masterGraph.node[group]["graph"] = nx.Graph() masterGraph.node[group]["angle_scale"] = calib_info[group]["angle_scale"] if calib_info[group].get("ar_markers") is None: calib_info[group]["ar_markers"] = [] if calib_info[group].get("hydras") is None: calib_info[group]["hydras"] = [] if calib_info[group].get("ps_markers") is None: calib_info[group]["ps_markers"] = [] if calib_info[group].get("master_group") is not None: masterGraph.node[group]["master"] = 1 masterGraph.node[group]["angle_scale"] = 0 masterGraph.node[group]["markers"] = ( calib_info[group]["ar_markers"] + calib_info[group]["hydras"] + calib_info[group]["ps_markers"] ) masterGraph.node[group]["calib_info"] = calib_info[group] ar_markers.extend(calib_info[group]["ar_markers"]) hydras.extend(calib_info[group]["hydras"]) ps_markers.extend(calib_info[group]["ps_markers"]) if freq is not None: wait_time = 5 print "Waiting for %f seconds before collecting data." % wait_time time.sleep(wait_time) sleeper = rospy.Rate(30) count = 0 while True: if freq is None: raw_input(colorize("Iteration %d: Press return when ready to capture transforms." % count, "red", True)) else: print colorize("Iteration %d" % count, "red", True) avg_tfms = {} for j in xrange(n_avg): print colorize("\tGetting averaging transform : %d of %d ..." % (j, n_avg - 1), "blue", True) tfms = {} tfms.update(get_ar_transforms(ar_markers, parent_frame)) tfms.update(get_hydra_transforms(hydras, parent_frame)) tfms.update(get_phasespace_transforms(ps_markers, parent_frame)) pot_angle = get_potentiometer_angle() for marker in tfms: if marker not in avg_tfms: avg_tfms[marker] = [] avg_tfms[marker].append(tfms[marker]) sleeper.sleep() for marker in avg_tfms: avg_tfms[marker] = utils.avg_transform(avg_tfms[marker]) update_groups_from_observations(masterGraph, avg_tfms, pot_angle) count += 1 if is_ready(masterGraph, min_obs): if freq: break elif not yes_or_no("Enough data has been gathered. Would you like to gather more data anyway?"): break print "Finished gathering data in %d iterations." % count print "Calibrating for optimal transforms between markers..." G_opt = compute_relative_transforms(masterGraph, min_obs=min_obs) print "Finished calibrating." return G_opt
def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef): N,D = x_na.shape M = nr_ma.shape[0] E = N + 4*M F = E - M Q = N + 3*M - 4 s = .1 # tetrahedron sidelength (meters) u = 1/(2*np.sqrt(2)) tetra_pts = [] for pt in nr_ma: tetra_pts.append(s*np.r_[-.5, 0, -u]+pt) tetra_pts.append(s*np.r_[+.5, 0, -u]+pt) tetra_pts.append(s*np.r_[0, -.5, +u]+pt) tetra_pts.append(s*np.r_[0, +.5, +u]+pt) x_ea = np.r_[x_na, tetra_pts] badsub_ex = np.c_[x_ea, np.ones((E,1)), np.r_[np.zeros((N,M)), np.repeat(np.eye(M), 4, axis=0)]] lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3) w_eg = np.r_[w_ng, np.zeros((4*M, D))] assert badsub_ex.shape[0] >= badsub_ex.shape[1] _u,_s,_vh = np.linalg.svd(badsub_ex, full_matrices=True) assert badsub_ex.shape[1] == _s.size N_eq = _u[:,badsub_ex.shape[1]:] # null of data assert N_eq.shape == (E,Q) assert E == N + 4*M assert F == Q + 4 # e is number of kernels # q is number of nonrigid dofs # f is total number of dofs K_ee = tps_kernel_matrix(x_ea) K_ne = K_ee[:N, :] Q_nf = np.c_[x_na, np.ones((N,1)),K_ne.dot(N_eq)] QQ_ff = np.dot(Q_nf.T, Q_nf) Bend_ff = np.zeros((F,F)) Bend_ff[4:, 4:] = - N_eq.T.dot(K_ee.dot(N_eq)) # K_qq assert Q_nf.shape == (N, F) assert w_eg.shape == (E, D) n_iter=40 for i in xrange(n_iter): # if plotting and i%plotting==0: # import lfd.registration as lr # lr.Globals.setup() # def eval_partial(x_ma): # return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) # lr.plot_orig_and_warped_clouds(eval_partial, x_na, y_ng, res=.008) X_fg = np.r_[lin_ag, trans_g[None,:], N_eq.T.dot(w_eg)] res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True) if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'), if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost) Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma, lin_ag, trans_g, w_eg, x_ea, return_tuple=True) nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea) fullstep_fg = np.empty((F,D)) for g in xrange(D): J_zf = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zeg[:,g::D].dot(N_eq)] JJ_ff = np.dot(J_zf.T, J_zf) A_ff = nr_coef*JJ_ff + QQ_ff + bend_coef*Bend_ff X0 = X_fg[:,g] B_f = nr_coef*np.dot(J_zf.T, np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot(y_ng[:,g]) fullstep_fg[:,g] = np.linalg.solve(A_ff,B_f) - X_fg[:,g] cost_improved = False for stepsize in 3.**np.arange(0,-10,-1): cand_X_fg = X_fg + fullstep_fg*stepsize cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[D], N_eq.dot(cand_X_fg[D+1:]) fval_cand = tps_nr_cost_eval_general(cand_lin_ag, cand_trans_g, cand_w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=False) if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand) if fval_cand < fval: cost_improved = True break if not cost_improved: if VERBOSE: print "couldn't improve objective" break lin_ag = cand_lin_ag trans_g = cand_trans_g w_eg = cand_w_eg return lin_ag, trans_g, w_eg, x_ea
def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"): N,D = x_na.shape lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3) #return lin_ag, trans_g, w_ng ##for testing that it takes one step when nonrigidity cost is zero: #lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0) #res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True) #print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval) #lin_ag += np.random.randn(*lin_ag.shape)*5 #res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True) #print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval) _u,_s,_vh = np.linalg.svd(np.c_[x_na, np.ones((N,1))], full_matrices=True) N_nq = _u[:,4:] # null of data #w_ng = N_nq.dot(N_nq.T.dot(w_ng)) K_nn = tps_kernel_matrix(x_na) Q_nn = np.c_[x_na, np.ones((N,1)),K_nn.dot(N_nq)] QQ_nn = np.dot(Q_nn.T, Q_nn) Bend_nn = np.zeros((N,N)) Bend_nn[4:, 4:] = - N_nq.T.dot(K_nn.dot(N_nq)) n_iter=60 for i in xrange(n_iter): X_ng = np.r_[lin_ag, trans_g[None,:], N_nq.T.dot(w_ng)] res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True) if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'), if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost) Jl_zcg, Jt_zg, Jw_zng = tps_nr_grad(nr_ma, lin_ag, trans_g, w_ng, x_na, return_tuple=True) nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na) if method == "newton": fullstep_ng = np.empty((N,D)) for g in xrange(D): J_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)] JJ_nn = np.dot(J_zn.T, J_zn) A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn X0 = X_ng[:,g] B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g]) fullstep_ng[:,g] = np.linalg.solve(A,B) - X_ng[:,g] elif method == "gradient": # def eval_partial(cand_X_ng): # cand_X_ng = cand_X_ng.reshape(-1,3) # cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:]) # fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef) # return fval_cand # def eval_partial2(cand_X_ng): # return ((Q_nn.dot(X_ng) - y_ng)**2).sum() # def eval_partial3(cand_X_ng): # cand_X_ng = cand_X_ng.reshape(-1,3) # cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:]) # return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g, cand_w_ng, x_na))**2).sum() grad_ng = np.empty((N,D)) for g in xrange(D-1,-1,-1): Jnr_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)] grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \ + 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \ + 2 * bend_coef * Bend_nn.dot(X_ng[:,g]) #assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng)) #assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng)) #grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3) fullstep_ng = -grad_ng #assert np.allclose(grad0_ng, grad_ng) cost_improved = False for stepsize in 3.**np.arange(0,-10,-1): cand_X_ng = X_ng + fullstep_ng*stepsize cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:]) fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef) if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand) if fval_cand < fval: cost_improved = True break if not cost_improved: if VERBOSE: print "couldn't improve objective" break lin_ag = cand_lin_ag trans_g = cand_trans_g w_ng = cand_w_ng return lin_ag, trans_g, w_ng
def get_transform_freq (grabber, marker, freq=0.5, n_avg=10, n_tfm=3, print_shit=False): """ Stores data at frequency provided. Switch on phasespace before this. """ tfms_ar = [] tfms_ph = [] I_0 = np.eye(4) I_0[3,3] = 0 i = 0 wait_time = 5 print "Waiting for %f seconds before collecting data."%wait_time time.sleep(wait_time) avg_freq = 30 while i < n_tfm: print "Transform %i"%(i+1) j = 0 ar_tfms = [] ph_tfms = [] while j < n_avg: rgb, depth = grabber.getRGBD() ar_tfm = get_ar_transform_id(depth, rgb, marker) ph_tfm = ph.marker_transform(0,1,2, ph.get_marker_positions()) if ar_tfm is None: print colorize("Could not find AR marker %i."%marker,"red",True) continue if ph_tfm is None: print colorize("Could not correct phasespace markers.", "red", True) continue print colorize("Got transform %i for averaging."%(j+1), "blue", True) ar_tfms.append(ar_tfm) ph_tfms.append(ph_tfm) j += 1 time.sleep(1/avg_freq) ar_avg_tfm = utils.avg_transform(ar_tfms) ph_avg_tfm = utils.avg_transform(ph_tfms) if print_shit: print "\n ar: " print ar_avg_tfm print ar_avg_tfm.dot(I_0).dot(ar_avg_tfm.T) print "ph: " print ph_avg_tfm print ph_avg_tfm.dot(I_0).dot(ph_avg_tfm.T) tfms_ar.append(ar_avg_tfm) tfms_ph.append(ph_avg_tfm) i += 1 time.sleep(1/freq) print "Found %i transforms. Calibrating..."%n_tfm Tas = ss.solve4(tfms_ar, tfms_ph) print "Done." T_cps = [tfms_ar[i].dot(Tas).dot(np.linalg.inv(tfms_ph[i])) for i in xrange(len(tfms_ar))] return utils.avg_transform(T_cps)
def get_transforms(arm, hydra, n_tfm , n_avg): """ Returns a tuple of two list of N_TFM transforms, each the average of N_AVG transforms corresponding to the left/ right arm of the pr2 and the hydra paddle of the HYDRA ('right' or 'left') side. Return gripper_tfms, hydra_tfms, kinect_tfms, head_tfms """ pr2_frame = 'base_footprint' assert arm == 'right' or 'left' arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm] head_frame = 'head_plate_frame' hydra_frame = 'hydra_base' assert hydra =='right' or hydra=='left' paddle_frame = 'hydra_%s' % hydra tf_sub = tf.TransformListener() I_0 = np.eye(4) I_0[3, 3] = 0 gripper_tfms = [] hydra_tfms = [] kinect_tfms = [] head_tfms = [] ar_markers = ARMarkersRos('/camera1_') time.sleep(3) marker_id = 11 i = 0 while i < n_tfm: raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm-1), "red", True)) ## transforms which need to be averaged. gtfms = [] htfms = [] ktfms = [] ptfms = [] sleeper = rospy.Rate(30) collect_enough_data = True for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...'%(j, n_avg-1), "blue", True) kinect_tfm = ar_markers.get_marker_transforms(time_thresh=0.5) print kinect_tfm if kinect_tfm == {}: print "Lost sight of AR marker. Breaking..." collect_enough_data = False break ptrans, prot = tf_sub.lookupTransform(pr2_frame, head_frame, rospy.Time(0)) ptfms.append(conversions.trans_rot_to_hmat(ptrans,prot)) gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0)) gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot)) htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0)) htfms.append(conversions.trans_rot_to_hmat(htrans, hrot)) ktrans, krot = conversions.hmat_to_trans_rot(kinect_tfm[marker_id]) ktfms.append(conversions.trans_rot_to_hmat(ktrans, krot)) sleeper.sleep() if collect_enough_data: gripper_tfms.append(avg_transform(gtfms)) hydra_tfms.append(avg_transform(htfms)) kinect_tfms.append(avg_transform(ktfms)) head_tfms.append(avg_transform(ptfms)) i += 1 return (gripper_tfms, hydra_tfms, kinect_tfms, head_tfms)
def get_synced_data(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera, plot=False): """ Returns two lists : (1) estimate of the hydra-sensor from hydra-base. (2) estimate of the hydra-sensor from the camera in hydra-bases frame. """ ## The hydra and camera data streams are not synced (some delay issues), therefore ## first remove that shift using correlation shift. _, _, _, ar1_strm, ar2_strm, hy_strm = relative_time_streams(data_file, lr, freq, single_camera) ## run the kalman filter on just the camera-data. nsteps, tmin, F_means, S, A,R = run_kalman_filter(data_file, lr, freq, use_spline, False, single_camera) X_kf = np.array(F_means) X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T if use_spline: smooth_hy = (t for t in fit_spline_to_stream(hy_strm, nsteps)) else: smooth_hy = hy_strm indices_ar1 = [] indices_ar2 = [] indices_hy = [] Ts_ar1 = [] Ts_ar2 = [] Ts_hy = [] for i in xrange(nsteps): ar1_est = soft_next(ar1_strm) ar2_est = soft_next(ar2_strm) hy_est = soft_next(smooth_hy) if ar1_est != None: Ts_ar1.append(ar1_est) indices_ar1.append(i) if ar2_est != None: Ts_ar2.append(ar2_est) indices_ar2.append(i) if hy_est != None: Ts_hy.append(hy_est) indices_hy.append(i) X_ar1 = state_from_tfms_no_velocity(Ts_ar1).T X_ar2 = state_from_tfms_no_velocity(Ts_ar2).T X_hy = state_from_tfms_no_velocity(Ts_hy).T ## shift the ar1 data to sync it up with the hydra data: if customized_shift != None: shift = customized_shift else: shift = correlation_shift(X_kf, X_hy) ## shift the indices of ar1 by the SHIFT calculated above: indices_hy = [idx + shift for idx in indices_hy] indices_hy = [x for x in indices_hy if 0<= x and x < nsteps] Ts_hy_matching = [] Ts_ar1_matching = [] for i,x in enumerate(indices_ar1): try: idx = indices_hy.index(x) Ts_hy_matching.append(Ts_hy[idx]) Ts_ar1_matching.append(Ts_ar1[i]) except: pass print colorize("Found %s corresponding data points b/w camera and hydra." % colorize("%d"%len(Ts_ar1_matching), "red", True), "blue", True) if plot: X_ar_matching = state_from_tfms_no_velocity(Ts_ar1_matching).T X_hy_matching = state_from_tfms_no_velocity(Ts_hy_matching).T plot_kalman_core(X_ar_matching, X_hy_matching, None, None, None, None, None, None, 'fs') plt.show() return (Ts_hy_matching, Ts_ar1_matching)
def get_transforms(marker, hydra, n_tfm , n_avg): """ Returns a tuple of two list of N_TFM transforms, each the average of N_AVG transforms corresponding to the AR marker with ID = MARKER and the hydra paddle of the HYDRA ('right' or 'left') side. """ camera_frame = 'camera1_link' hydra_frame = 'hydra_base' marker_frame = 'ar_marker%d_camera1' % marker assert hydra == 'right' or hydra == 'left' paddle_frame = 'hydra_%s' % hydra tf_sub = tf.TransformListener() I_0 = np.eye(4) I_0[3, 3] = 0 ar_tfms = [] hydra_tfms = [] for i in xrange(n_tfm + 1): raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm), "red", True)) # # transforms which need to be averaged. m_ts = np.empty((0, 3)) m_qs = np.empty((0, 4)) h_ts = np.empty((0, 3)) h_qs = np.empty((0, 4)) for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True) mtrans, mrot, htrans, hrot = None, None, None, None sleeper = rospy.Rate(30) while htrans == None: # now = rospy.Time.now() # tf_sub.waitForTransform(camera_frame, marker_frame, now) # mtrans, mrot = tf_sub.lookupTransform(marker_frame, camera_frame, rospy.Time(0)) # (htrans,hrot) = tf_sub.lookupTransform(paddle_frame, hydra_frame, rospy.Time(0)) mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0)) htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0)) sleeper.sleep() # try: # tf_sub.waitForTransform(hydra_frame, paddle_frame, now, rospy.Duration(0.05)) # except (tf.LookupException): # continue m_ts = np.r_[m_ts, np.array(mtrans, ndmin=2)] h_ts = np.r_[h_ts, np.array(htrans, ndmin=2)] m_qs = np.r_[m_qs, np.array(mrot, ndmin=2)] h_qs = np.r_[h_qs, np.array(hrot, ndmin=2)] mtrans_avg = np.sum(m_ts, axis=0) / n_avg mrot_avg = avg_quaternions(m_qs) htrans_avg = np.sum(h_ts, axis=0) / n_avg hrot_avg = avg_quaternions(h_qs) # ar_tfm = tf_sub.fromTranslationRotation(mtrans_avg, mrot_avg) # h_tfm = tf_sub.fromTranslationRotation(htrans_avg, hrot_avg) ar_tfm = conversions.trans_rot_to_hmat(mtrans_avg, mrot_avg) h_tfm = conversions.trans_rot_to_hmat(htrans_avg, hrot_avg) print "\nar:" print ar_tfm print ar_tfm.dot(I_0).dot(ar_tfm.T) print "h:" print h_tfm print h_tfm.dot(I_0).dot(h_tfm.T), "\n" # ar_tfm = conversions.trans_rot_to_hmat(mtrans,mrot) # h_tfm = conversions.trans_rot_to_hmat(htrans,hrot) # print "\nar:" # print ar_tfm # print ar_tfm.dot(I_0).dot(ar_tfm.T) # print "h:" # print h_tfm # print h_tfm.dot(I_0).dot(h_tfm.T), "\n" ar_tfms.append(ar_tfm) hydra_tfms.append(h_tfm) return (ar_tfms, hydra_tfms)
plt.clf() xpf_t_2 = xpf_2.T xh_t_2 = xh_2.T for i in xrange(6): j = i+3 if i > 2 else i plt.subplot(3,2,i+1) plt.plot(xpf_t_2[j,:], label='pr2') plt.plot(xh_t_2[j,:], label='hydra') plt.plot(X_est[j, :], label='estimate') plt.ylabel(axlabels[i]) plt.legend() plt.show(block=False) orig_cal_pose_error = ec.calc_pose_error(A, B) print colorize("original camera calibration pose error", "blue", True) print "mean", np.mean(orig_cal_pose_error, axis=0) print "std", np.std(orig_cal_pose_error, axis=0) print colorize("--------------------------------------", "blue", True) # systematic and GP corrected robot poses gp_pose_error = ec.calc_pose_error(D, ests) print colorize("systematic and GP corrected pose error", "blue", True) print "mean", np.mean(gp_pose_error, axis=0) print "std", np.std(gp_pose_error, axis=0) print colorize("--------------------------------------", "blue", True)
def get_transform_ros(marker, n_tfm, n_avg, freq=None): """ Returns a tuple of two list of N_TFM transforms, each the average of N_AVG transforms corresponding to the AR marker with ID = MARKER and the phasespace markers. """ camera_frame = 'camera_depth_optical_frame' marker_frame = 'ar_marker_%d'%marker ps_frame = 'ps_marker_transform' tf_sub = tf.TransformListener() I_0 = np.eye(4) I_0[3,3] = 0 ar_tfms = [] ph_tfms = [] wait_time = 5 print "Waiting for %f seconds before collecting data."%wait_time time.sleep(wait_time) sleeper = rospy.Rate(30) for i in xrange(n_tfm+1): if freq is None: raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm), "red", True)) else: print colorize("Transform %d of %d."%(i, n_tfm), "red", True) ## transforms which need to be averaged. ar_tfm_avgs = [] ph_tfm_avgs = [] for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True) mtrans, mrot, ptrans, prot = None, None, None, None while ptrans == None or mtrans == None: mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0)) ptrans, prot = tf_sub.lookupTransform(ph.PHASESPACE_FRAME, ps_frame, rospy.Time(0)) sleeper.sleep() ar_tfm_avgs.append(conversions.trans_rot_to_hmat(mtrans,mrot)) ph_tfm_avgs.append(conversions.trans_rot_to_hmat(ptrans,prot)) ar_tfm = utils.avg_transform(ar_tfm_avgs) ph_tfm = utils.avg_transform(ph_tfm_avgs) # print "\nar:" # print ar_tfm # print ar_tfm.dot(I_0).dot(ar_tfm.T) # print "h:" # print ph_tfm # print ph_tfm.dot(I_0).dot(ph_tfm.T), "\n" ar_tfms.append(ar_tfm) ph_tfms.append(ph_tfm) if freq is not None: time.sleep(1/freq) print "Found %i transforms. Calibrating..."%n_tfm Tas = ss.solve4(ar_tfms, ph_tfms) print "Done." T_cps = [ar_tfms[i].dot(Tas).dot(np.linalg.inv(ph_tfms[i])) for i in xrange(len(ar_tfms))] return utils.avg_transform(T_cps)
def make_shell_calls(cmd_params): test_command, demo_name, perturb_name = cmd_params shell_cmd = test_command%(args.init_state_h5, args.demo_type, demo_name, perturb_name, state_dir) print colorize("Calling:\n\t"+shell_cmd, "green", True) return os.system(shell_cmd)