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 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 make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0): "do ik and then fill in the points where ik failed" n = len(xyzs) assert len(quats) == n robot = manip.GetRobot() joint_inds = manip.GetArmIndices() robot.SetActiveDOFs(joint_inds) orig_joint = robot.GetActiveDOFValues() joints = [] inds = [] for i in xrange(0,n): mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i]) joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options) if joint is not None: joints.append(joint) inds.append(i) robot.SetActiveDOFValues(joint) robot.SetActiveDOFValues(orig_joint) LOG.info("found ik soln for %i of %i points",len(inds), n) if len(inds) > 2: joints2 = mu.interp2d(np.arange(n), inds, joints) return joints2, inds else: return np.zeros((n, len(joints))), []
def save_calibration(self, cfile): """ Save the transforms and the gripper data from this current calibration. This assumes that the entire calibration data is stored in this class. """ calib_data = {} gripper_data = {} if self.gripper_lite: for (lr, gr) in self.grippers.items(): gripper_data[lr] = {'ar': gr.get_ar_marker(), 'hydra': gr.get_hydra_marker(), 'tfms': gr.get_saveable_transforms()} else: for (lr, gr) in self.grippers.items(): gripper_data[lr] = gr.transform_graph calib_data['grippers'] = gripper_data calib_transforms = [] for (parent, child) in self.transforms: tfm = {} tfm['parent'] = parent tfm['child'] = child (trans, rot) = self.transforms[parent, child] tfm['tfm'] = conversions.trans_rot_to_hmat(trans, rot) calib_transforms.append(tfm) calib_data['transforms'] = calib_transforms file_name = osp.join(calib_files_dir, cfile) with open(file_name, 'w') as fh: cPickle.dump(calib_data, fh)
def get_hydra_transforms(parent_frame, hydras = None): """ Returns hydra transform in hydra base frame. hydras is a list which contains 'left', 'right' or both """ global tf_listener, hydra_initialized if not hydra_initialized: if tf_listener is None: tf_listener = tf.TransformListener() hydra_initialized = True print "HYDRA INITIALIZED" if hydras is None: hydras = ['left','right'] hydra_transforms = {} for hydra in hydras: hydra_frame = 'hydra_%s'%hydra try: trans, rot = tf_listener.lookupTransform(parent_frame, hydra_frame, rospy.Time(0)) hydra_transforms[hydra] = conversions.trans_rot_to_hmat(trans, rot) except (tf.LookupException, tf.ExtrapolationException, tf.ConnectivityException): continue return hydra_transforms
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_transform_frame(self, cam, frame): """ Gets transform of cam in frame's frame. Stores transforms when found. Assumes offset does not change between frame and camera. """ if frame not in self.stored_tfms: trans, rot = self.tf_listener.lookupTransform(frame, self.parent_frame, rospy.Time(0)) self.stored_tfms[frame] = conversions.trans_rot_to_hmat(trans, rot) return self.stored_tfms[frame].dot(self.get_camera_transform(0, cam))
def get_transform_frames (parent_frame, child_frame): """ Gets transform between frames. """ if tf_initialized is False: if tf_listener is None: tf_listener = tf.TransformListener() tf_initialized = True print "TF INITIALIZED" trans, quat = tf_listener.lookupTransform(parent_frame, child_frame, rospy.Time(0)) return conversions.trans_rot_to_hmat(trans, quat)
def get_all_transforms(self): ''' Return [{'parent': parent, 'child': child, 'tfm': hmat}] ''' result_tfms = [] for (parent, child) in self.transforms: tfm = {'parent': parent, 'child': child} (trans, rot) = self.transforms[parent, child] tfm['tfm'] = conversions.trans_rot_to_hmat(trans, rot) result_tfms.append(tfm) return result_tfms
def get_ar_transforms(markers, parent_frame): """ Takes in a list of @markers (AR marker ids) and a @parent_frame. Returns a dict of transforms with keys as found marker ids and values as transforms. """ if markers is None: return {} ar_tfms = {} for marker in markers: try: trans, rot = tf_listener.lookupTransform(parent_frame, "ar_marker_%d" % marker, rospy.Time(0)) ar_tfms[marker] = conversions.trans_rot_to_hmat(trans, rot) except: pass return ar_tfms
def get_camera_transforms(self): ''' Return a dictionary {(c1, c2): tf1, (c1', c2'): tf2} tf1 is a dict with 'parent', 'child' and 4x4 'tfm' ''' result_tfms = {} for (parent, child) in self.transforms: if 'camera' in parent and 'camera' in child: c1 = int(parent[6]) - 1 # assume parent is something like 'camera1' c2 = int(child[6]) - 1 # assume child is something like 'camera1' tfm = {'parent': parent, 'child': child} (trans, rot) = self.transforms[parent, child] tfm['tfm'] = conversions.trans_rot_to_hmat(trans, rot) result_tfms[c1, c2] = tfm return result_tfms
def get_robot_kinect_transform(): global tf_listener tfm_base_kinect = None for i in xrange(5): try: now = rospy.Time.now() tf_listener.waitForTransform('base_footprint', 'camera_link', now, rospy.Duration(5.0)) (trans, rot) = tf_listener.lookupTransform('base_footprint', 'camera_link', now) tfm_base_kinect = conversions.trans_rot_to_hmat(trans, rot) print tfm_base_kinect yellowprint("Got the transform for PR2") break except (tf.Exception, tf.LookupException, tf.ConnectivityException): yellowprint("Failed attempt.") return tfm_base_kinect
def get_hydra_transforms(parent_frame, hydras=None): """ Returns hydra transform in hydra base frame. hydras is a list which contains 'left', 'right' or both """ global tf_l, hydra_initialized if not hydra_initialized: if rospy.get_name() == '/unnamed': rospy.init_node('hydra_tfm') tf_l = tf.TransformListener() hydra_initialized = False if hydras is None: hydras = ['left','right'] hydra_transforms = {} for hydra in hydras: hydra_frame = 'hydra_%s_pivot'%hydra trans, rot = tf_l.lookupTransform(parent_frame, hydra_frame, rospy.Time(0)) hydra_transforms[hydra] = conversions.trans_rot_to_hmat(trans, rot) return hydra_transforms
def get_transform (parent_frame, child_frame, n_attempts = None): """ Wrapper for getting tf transform. """ global tfl, tf_sleeper if tfl is None: tfl = tf.TransformListener() tf_sleeper = rospy.Rate(30) while n_attempts is None or n_attempts > 0: try: trans, rot = tfl.lookupTransform(parent_frame, child_frame, rospy.Time(0)) tfm = conversions.trans_rot_to_hmat(trans, rot) return tfm except (tf.LookupException, tf.ExtrapolationException, tf.ConnectivityException): if n_attempts is not None: n_attempts -= 1 tf_sleeper.sleep() redprint("Unable to find transform from %s to %s."%(parent_frame, child_frame)) return None
def save_hydra_only (demo_type, demo_name, save_file=None): """ Save hydra only data. """ # Temp file to show that data is already being extracted demo_dir = osp.join(demo_files_dir, demo_type, demo_name) with open(osp.join(demo_dir, demo_names.extract_hydra_data_temp),'w') as fh: fh.write('Extracting...') bag_file = osp.join(demo_dir, demo_names.bag_name) calib_file_path = osp.join(demo_dir, demo_names.calib_name) data = {} c1_frame = 'camera1_link' hydra_frame = 'hydra_base' tfm_c1_h = None with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh) bag = rosbag.Bag(bag_file) for tfm in calib_data['transforms']: if tfm['parent'] == c1_frame or tfm['parent'] == '/' + c1_frame: if tfm['child'] == hydra_frame or tfm['child'] == '/' + hydra_frame: tfm_c1_h = nlg.inv(tfm_link_rof).dot(tfm['tfm']) if tfm_c1_h is None: redprint("Calibration does not have hydra transform.") return data['T_cam2hbase'] = tfm_c1_h # If demo.data file already exists, don't do extra work. found_hydra_data = False found_pot_data = False all_data_file = osp.join(demo_dir, demo_names.data_name) if osp.isfile(all_data_file): yellowprint ("%s already exists for %s. Creating hydra file from this info."%(demo_names.data_name,demo_name)) with open(all_data_file,'r') as fh: all_data = cPickle.load(fh) for lr in all_data: if lr in ['lr']: data[lr] = {} if all_data[lr].get('hydra'): data[lr]['hydra'] = all_data[lr]['hydra'] found_hydra_data = True if all_data[lr].get('pot_angles'): data[lr]['pot_angles'] = all_data[lr]['pot_angles'] found_pot_data = True if found_hydra_data and found_pot_data: if save_file is None: save_file = demo_names.hydra_data_name save_filename = osp.join(demo_dir, save_file) with open(save_filename, 'w') as sfh: cPickle.dump(data, sfh) return if not calib_data.get('grippers'): redprint("Gripper not found.") return grippers = {} for lr,gdata in calib_data['grippers'].items(): gr = gripper_lite.GripperLite(lr, gdata['ar'], trans_marker_tooltip=gripper_trans_marker_tooltip[lr]) gr.reset_gripper(lr, gdata['tfms'], gdata['ar'], gdata['hydra']) grippers[lr] = gr data[lr] ={'hydra':[], 'pot_angles':[], 'T_tt2hy': gr.get_rel_transform('tool_tip', gr.hydra_marker)} if not found_hydra_data: if verbose: yellowprint('Hydra') lr_long = {'l':'left','r':'right'} for (_, msg, _) in bag.read_messages(topics=['/tf']): hyd_tfm = {} found = '' for tfm in msg.transforms: if found in ['lr','rl']: break for lr in grippers: if lr in found: continue elif tfm.header.frame_id == '/' + hydra_frame and tfm.child_frame_id == '/hydra_'+lr_long[lr]: t,r = tfm.transform.translation, tfm.transform.rotation trans = (t.x,t.y,t.z) rot = (r.x, r.y, r.z, r.w) hyd_tfm = tfm_c1_h.dot(conversions.trans_rot_to_hmat(trans, rot)) stamp = tfm.header.stamp.to_sec() tt_tfm = grippers[lr].get_tooltip_transform(lr_long[lr], hyd_tfm) data[lr]['hydra'].append((tt_tfm, stamp)) found += lr if found and verbose: blueprint("Got hydra readings %s at time %f"%(found,stamp)) if not found_pot_data: if verbose: yellowprint('Potentiometer readings') for lr in grippers: for (_, msg, ts) in bag.read_messages(topics=['/%s_pot_angle'%lr]): angle = msg.data stamp = ts.to_sec() data[lr]['pot_angles'].append((angle, stamp)) if verbose: blueprint("Got a %s potentiometer angle of %f at time %f"%(lr,angle, stamp)) if verbose: for lr in 'lr': yellowprint("Gripper %s:"%lr) yellowprint("Found %i transforms from hydra"%len(data[lr]['hydra'])) yellowprint("Found %i potentiometer readings"%len(data[lr]['pot_angles'])) if save_file is None: save_file = demo_names.hydra_data_name save_filename = osp.join(demo_dir, save_file) with open(save_filename, 'w') as sfh: cPickle.dump(data, sfh) yellowprint("Saved %s."%demo_names.hydra_data_name) os.remove(osp.join(demo_dir, demo_names.extract_hydra_data_temp))
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)
#tool_tfms, model_tfms = get_tool_model(vals.n_tfm, vals.n_avg) tool_tfms = get_tool_model(vals.n_tfm, vals.n_avg) tool_to_model_tfms = [] for i in xrange(vals.n_tfm): tool = tool_tfms[i] model = model_tfms[i] tool_inv = nlg.inv(tool) tool_to_model_tfm = tool_inv.dot(model) tool_to_model_tfms.append(tool_to_model_tfm) trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in tool_to_model_tfms] trans = np.asarray([trans for (trans, rot) in trans_rots]) avg_trans = np.sum(trans,axis=0)/trans.shape[0] rots = [rot for (trans, rot) in trans_rots] avg_rot = avg_quaternions(np.array(rots)) tool_to_model_tfm_avg = conversions.trans_rot_to_hmat(avg_trans, avg_rot) print tool_to_model_tfm_avg if vals.publish_tf: publish_tf(tool_to_model_tfm_avg, 'l_gripper_tool_frame', 'gripper_model_frame')
listener = tf.TransformListener() time.sleep(3) while(i < args.n_tfm): rgb = colorStream.readFrame() #cv2.imshow("rgb", rgb.data) depth = depthStream.readFrame() #cv2.imshow("depth", cmap[np.fmin((depth.data*.064).astype('int'), 255)]) 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)) ar_tfm = get_ar_transform_id(depth.data, rgb.data, 2) if ar_tfm == None: print "got none" continue tool_tfm = conversions.trans_rot_to_hmat(tool_trans, tool_quat) head_tfm = conversions.trans_rot_to_hmat(head_trans, head_quat) #print head_tfm #print tool_tfm tool_tfms.append(tool_tfm) head_tfms.append(head_tfm) ar_tfms.append(ar_tfm) #trans, rot = conversions.hmat_to_trans_rot(tfm) print i i = i+1 ar_in_base_tfms = [] for i in xrange(len(ar_tfms)): head_frame_ar = tfm_head_kinect.dot(ar_tfms[i]) base_frame_ar = head_tfms[i].dot(head_frame_ar)
k1_t, k1_q = conversions.hmat_to_trans_rot(tfm1) k1_ts = np.r_[k1_ts, np.array(k1_t, ndmin=2)] k1_qs = np.r_[k1_qs, np.array(k1_q, ndmin=2)] rgb2 = cs2.readFrame() depth2 = ds2.readFrame() tfm2 = get_ar_transform_id(depth2.data, rgb2.data, 2) #print tfm #trans, rot = conversions.hmat_to_trans_rot(tfm) print tfm2 if tfm2 == None: print "got none" continue k2_t, k2_q = conversions.hmat_to_trans_rot(tfm2) k2_ts = np.r_[k2_ts, np.array(k2_t, ndmin=2)] k2_qs = np.r_[k2_qs, np.array(k2_q, ndmin=2)] print i i = i+1 #except: # print "Warning: Problem with ar" # pass #cv2.waitKey(30) k1_trans_avg = np.sum(k1_ts, axis=0) / args.n_tfm k1_rot_avg = avg_quaternions(k1_qs) k1_tfm = conversions.trans_rot_to_hmat(k1_trans_avg, k1_rot_avg) print k1_tfm #except KeyboardInterrupt: # print "got Control-C"
sleeper.sleep() if __name__ == '__main__': rospy.init_node('calib_hydra_pr2') parser = argparse.ArgumentParser(description="Hydra Kinect Calibration") parser.add_argument('--arm', help="in {'right', 'left'} : the pr2 arm to track.", required=True) parser.add_argument('--hydra', help="in {'right', 'left'} : the hydra handle to track.", required=True) parser.add_argument('--n_tfm', help="number of transforms to use for calibration.", type=int, default=5) parser.add_argument('--n_avg', help="number of estimates of transform to use for averaging.", type=int, default=5) parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True) vals = parser.parse_args() arm_tfms, hydra_tfms = get_transforms(vals.arm, vals.hydra, vals.n_tfm, vals.n_avg) if vals.publish_tf: T_ms = solve_sylvester4(arm_tfms, hydra_tfms) T_chs = [arm_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(arm_tfms))] trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_chs] trans = np.asarray([trans for (trans, rot) in trans_rots]) avg_trans = np.sum(trans,axis=0)/trans.shape[0] rots = [rot for (trans, rot) in trans_rots] avg_rot = avg_quaternions(np.array(rots)) T_ch = conversions.trans_rot_to_hmat(avg_trans, avg_rot) print T_ch publish_tf(T_ch, 'base_footprint', 'hydra_base')
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 calibrate_cameras (): global tfm_pub, cameras, tf_listener tfm_base_kinect = get_robot_kinect_transform() if yes_or_no('Calibrate again?'): greenprint("Step 1. Calibrating multiple cameras.") cam_calib = CameraCalibrator(cameras) done = False while not done: cam_calib.calibrate(CAM_N_OBS, CAM_N_AVG) if cameras.num_cameras == 1: break if not cam_calib.calibrated: redprint("Camera calibration failed.") cam_calib.reset_calibration() else: tfm_reference_camera = cam_calib.get_transforms()[0] tfm_reference_camera['child'] = 'camera_link' tfm_base_reference = {} tfm_base_reference['child'] = transform['parent'] tfm_base_reference['parent'] = 'base_footprint' tfm_base_reference['tfm'] = nlg.inv(tfm_reference_camera['tfm'].dot(nlg.inv(tfm_base_kinect))) tfm_pub.add_transforms([tfm_base_reference]) if yes_or_no("Are you happy with the calibration? Check RVIZ."): done = True else: yellowprint("Calibrating cameras again.") cam_calib.reset_calibration() greenprint("Cameras calibrated.") else: tfm_pub.load_calibration('cam_pr2') if yes_or_no('Get PR2 Gripper?'): # Finding transform between PR2's gripper and the tool_tip marker_id = 1 camera_id = 1 n_avg = 100 tfms_camera_marker = [] for i in xrange(n_avg): tfms_camera_marker.append(cameras.get_ar_markers([marker_id], camera=camera_id)[marker_id]) time.sleep(0.03) tfm_camera_marker = utils.avg_transform(tfms_camera_marker) calib_file = 'calib_cam_hydra_gripper1' file_name = osp.join(calib_files_dir, calib_file) with open(file_name, 'r') as fh: calib_data = cPickle.load(fh) lr, graph = calib_data['grippers'].items()[0] gr = gripper.Gripper(lr, graph) assert 'tool_tip' in gr.mmarkers gr.tt_calculated = True tfm_marker_tooltip = gr.get_rel_transform(marker_id, 'tool_tip', 0) i = 10 tfm_base_gripper = None while i > 0: try: now = rospy.Time.now() tf_listener.waitForTransform('base_footprint', 'l_gripper_tool_frame', now, rospy.Duration(5.0)) (trans, rot) = tf_listener.lookupTransform('base_footprint', 'l_gripper_tool_frame', rospy.Time(0)) tfm_base_gripper = conversions.trans_rot_to_hmat(trans, rot) break except (tf.Exception, tf.LookupException, tf.ConnectivityException): yellowprint("Failed attempt.") i -= 1 pass if tfm_base_gripper is not None: tfm_gripper_tooltip = {'parent':'l_gripper_tool_frame', 'child':'pr2_lgripper_tooltip', 'tfm':nlg.inv(tfm_base_gripper).dot(tfm_base_kinect).dot(tfm_link_rof).dot(tfm_camera_marker).dot(tfm_marker_tooltip) } tfm_pub.add_transforms([tfm_gripper_tooltip]) greenprint("Gripper to marker found.") else: redprint("Gripper to marker not found.")
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)
def save_observations_rgbd(demo_type, demo_name, save_file=None): """ Extract data from all sensors and store into demo.data file. """ global setCalib # Temp file to show that data is already being extracted demo_dir = osp.join(demo_files_dir, demo_type, demo_name) with open(osp.join(demo_dir, demo_names.extract_data_temp),'w') as fh: fh.write('Extracting...') calib_file_path = osp.join(demo_dir, demo_names.calib_name) bag_file = osp.join(demo_dir, demo_names.bag_name) with open(osp.join(demo_dir, demo_names.camera_types_name)) as fh: camera_types = yaml.load(fh) with open(osp.join(demo_dir, demo_names.camera_models_name)) as fh: camera_models = yaml.load(fh) num_cameras = len(camera_types) video_dirs = {} for i in range(1, num_cameras + 1): video_dirs[i] = osp.join(demo_dir, demo_names.video_dir%i) c_frames = {} for i in range(1, num_cameras + 1): c_frames[i]= 'camera%i_link'%(i) hydra_frame = 'hydra_base' tfm_c1 = {i:None for i in range (1,num_cameras+1)} tfm_c1[1] = np.eye(4) tfm_c1_h = None with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh) bag = rosbag.Bag(bag_file) for tfm in calib_data['transforms']: if tfm['parent'] == c_frames[1] or tfm['parent'] == '/' + c_frames[1]: if tfm['child'] == hydra_frame or tfm['child'] == '/' + hydra_frame: tfm_c1_h = nlg.inv(tfm_link_rof).dot(tfm['tfm']) else: for i in range(2, num_cameras+1): if tfm['child'] == c_frames[i] or tfm['child'] == '/' + c_frames[i]: tfm_c1[i] = nlg.inv(tfm_link_rof).dot(tfm['tfm']).dot(tfm_link_rof) if tfm_c1_h is None or not all([tfm_c1[s] != None for s in tfm_c1]): redprint("Calibration does not have required transforms") return if not calib_data.get('grippers'): redprint("Gripper not found.") return grippers = {} data = {} data['T_cam2hbase'] = tfm_c1_h for lr,gdata in calib_data['grippers'].items(): gr = gripper_lite.GripperLite(lr, gdata['ar'], trans_marker_tooltip=gripper_trans_marker_tooltip[lr]) gr.reset_gripper(lr, gdata['tfms'], gdata['ar'], gdata['hydra']) grippers[lr] = gr data[lr] ={'hydra':[], 'pot_angles':[], 'T_tt2hy': gr.get_rel_transform('tool_tip', gr.hydra_marker)} ## place-holder for AR marker transforms: for i in xrange(num_cameras): data[lr]['camera%d'%(i+1)] = [] winname = 'cam_image' cam_counts = [] for i in range(1,num_cameras+1): if verbose: yellowprint('Camera%i'%i) if camera_types[i] == "rgbd": rgb_fnames, depth_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i]) else: rgb_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i], depth = False) cam_count = len(stamps) cam_counts.append(cam_count) if camera_types[i] == "rgbd": for ind in rgb_fnames: rgb = cv2.imread(rgb_fnames[ind]) if displayImages: cv2.imshow(winname, rgb) cv2.waitKey(1) assert rgb is not None depth = cv2.imread(depth_fnames[ind],2) assert depth is not None xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f) pc = ru.xyzrgb2pc(xyz, rgb, frame_id='', use_time_now=False) ar_tfms = get_ar_marker_poses(pc,track=True) if ar_tfms: if verbose: blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind]) for lr,gr in grippers.items(): ar = gr.get_ar_marker() if ar in ar_tfms: tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar])) data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind])) else: # if setCalib is None: # setCalib = rospy.ServiceProxy("setCalibInfo", SetCalibInfo) # reqCalib.camera_model = camera_models[i] # setCalib(reqCalib) # # if verbose: # yellowprint("Changed camera calibration parameters to model %s"%camera_models[i]) for ind in rgb_fnames: rgb = cv.LoadImage(rgb_fnames[ind]) if displayImages: cv.ShowImage(winname, rgb) cv.WaitKey(1) assert rgb is not None ar_tfms = get_ar_marker_poses(rgb,use_pc_service=False, model = camera_models[i], track=True) if ar_tfms: if verbose: blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind]) for lr,gr in grippers.items(): ar = gr.get_ar_marker() if ar in ar_tfms: tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar])) data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind])) found_hydra_data = False found_pot_data = False # If hydra_only.data file already exists, don't do extra work. hydra_data_file = osp.join(demo_dir, demo_names.hydra_data_name) if osp.isfile(hydra_data_file): yellowprint ("%s already exists for %s. Getting hydra data from this info."%(demo_names.hydra_data_name,demo_name)) with open(hydra_data_file,'r') as fh: hdata = cPickle.load(fh) for lr in hdata: if lr in 'lr': if hdata[lr].get('hydra'): data[lr]['hydra'] = hdata[lr]['hydra'] found_hydra_data = True if hdata[lr].get('pot_angles'): data[lr]['pot_angles'] = hdata[lr]['pot_angles'] found_pot_data = True if not found_hydra_data: if verbose: yellowprint('Hydra') lr_long = {'l':'left','r':'right'} for (_, msg, _) in bag.read_messages(topics=['/tf']): hyd_tfm = {} found = '' for tfm in msg.transforms: if found in ['lr','rl']: break for lr in grippers: if lr in found: continue elif tfm.header.frame_id == '/' + hydra_frame and tfm.child_frame_id == '/hydra_'+lr_long[lr]: t,r = tfm.transform.translation, tfm.transform.rotation trans = (t.x,t.y,t.z) rot = (r.x, r.y, r.z, r.w) hyd_tfm = tfm_c1_h.dot(conversions.trans_rot_to_hmat(trans, rot)) stamp = tfm.header.stamp.to_sec() tt_tfm = grippers[lr].get_tooltip_transform(lr_long[lr], hyd_tfm) data[lr]['hydra'].append((tt_tfm, stamp)) found += lr if found and verbose: blueprint("Got hydra readings %s at time %f"%(found,stamp)) if not found_pot_data: if verbose: yellowprint('Potentiometer readings') for lr in grippers: for (_, msg, ts) in bag.read_messages(topics=['/%s_pot_angle'%lr]): angle = msg.data stamp = ts.to_sec() data[lr]['pot_angles'].append((angle, stamp)) if verbose: blueprint("Got a %s potentiometer angle of %f at time %f"%(lr,angle, stamp)) if verbose: for lr in 'lr': yellowprint("Gripper %s:"%lr) for i in range(num_cameras): yellowprint("Found %i transforms out of %i rgb/rgbd images from camera%i"%(len(data[lr]['camera%i'%(i+1)]), cam_counts[i], i+1)) yellowprint("Found %i transforms from hydra"%len(data[lr]['hydra'])) yellowprint("Found %i potentiometer readings"%len(data[lr]['pot_angles'])) if save_file is None: save_file = demo_names.data_name save_filename = osp.join(demo_dir, save_file) with open(save_filename, 'w') as sfh: cPickle.dump(data, sfh) yellowprint("Saved %s."%demo_names.data_name) os.remove(osp.join(demo_dir, demo_names.extract_data_temp))