def save_clouds():
NUM_CAM = 2
cameras = RosCameras(NUM_CAM)
tfm_listener = tf.TransformListener()
fr1 = camera1_rgb_optical_frame
fr2 = camera2_rgb_optical_frame
c1, c2 = 0, 1
print "Waiting for service .."
rospy.wait_for_service('pcd_service')
pcdService = rospy.ServiceProxy("pcd_service", PCDData)
raw_input(colorize("Do not move the objects on the table now.", "green", True))
raw_input(colorize("Cover camera %i and hit enter!" % (c2 + 1), 'yellow', True))
pc1 = cameras.get_pointcloud(c1)
raw_input(colorize("Cover camera %i and hit enter!" % (c1 + 1), 'yellow', True))
pc2 = cameras.get_pointcloud(c2)
pc2 = ru.transformPointCloud2(pc2, tfm_listener, fr1, fr2)
req = PCDDataRequest()
req.pc = pc1
req.fname = "cloud1.pcd"
pcdService(req)
req.pc = pc2
req.fname = "cloud2.pcd"
pcdService(req)
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 calibrate (self, method='ar', use_icp=False, n_obs=10, n_avg=5):
print method
if self.num_cameras == 1:
redprint ("Only one camera. You don't need to calibrate.", True)
self.calibrated = True
self.cameras.calibrated = True
return
self.initialize_calibration(method)
n_effective_obs = 0
while n_effective_obs < n_obs:
yellowprint("Please hold still for a few seconds. Make sure the transforms look good on rviz.")
raw_input(colorize("Observation %d from %d. Press return when ready." % (n_effective_obs+1, n_obs), 'green', True))
if method == 'ar':
calibration_updated = self.process_observation_ar(n_avg)
elif method == 'cb':
calibration_updated = self.process_observation_cb()
else:
calibration_updated = self.process_observation_pycb()
if calibration_updated: n_effective_obs += 1
if method == 'pycb':
self.calibrated = self.finish_calibration_pycb()
else:
self.calibrated = self.finish_calibration(use_icp)
self.cameras.calibrated = self.calibrated
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 process_observation(self, n_avg=5):
"""
Store an observation of ar_marker and hydras.
"""
raw_input(colorize("Press return when ready to capture transforms.", "green", True))
ar_avg_tfm = []
hydra_avg_tfm = []
for j in xrange(n_avg):
print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)
ar_transforms = gmt.get_ar_markers_from_cameras(self.cameras, cams = [self.calib_camera], markers = [self.ar_marker])
hydra_transforms = gmt.get_hydra_transforms('hydra_base', [self.calib_hydra])
ar_avg_tfm.append(ar_transforms[self.ar_marker])
hydra_avg_tfm.append(hydra_transforms[self.ar_marker][self.calib_hydra])
self.ar_transforms.append(utils.avg_transform(ar_avg_tfm))
self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
def get_tfm_from_obs(self, hydra_marker, n_tfm=15, n_avg=30):
"""
Stores a bunch of readings from sensors.
"""
all_tfms = []
if rospy.get_name == '/unnamed':
rospy.init_node('gripper_calib')
sleeper = rospy.Rate(30)
i = 0
n_attempts_max = n_avg*2
while i < n_tfm:
raw_input(colorize("Getting transform %i out of %i. Hit return when ready."%(i, n_tfm-1), 'yellow', True))
j = 0
n_attempts_effective = 0;
hyd_tfms = []
ar_tfms = []
while j < n_attempts_max:
j += 1
ar_tfms_j = self.cameras.get_ar_markers(markers=[self.ar_marker]);
hyd_tfms_j = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=[hydra_marker]);
if not ar_tfms_j or not hyd_tfms_j:
if not ar_tfms_j:
yellowprint("Could not find required ar markers from " + str(self.ar_marker))
else:
yellowprint("Could not find required hydra transforms from " + str(hydra_marker))
continue
ar_tfms.append(ar_tfms_j[self.ar_marker])
hyd_tfms.append(hyd_tfms_j[hydra_marker])
blueprint('\tGetting averaging transform : %d of %d ...'%(n_attempts_effective, n_avg-1))
n_attempts_effective += 1
if n_attempts_effective == n_avg:
break
sleeper.sleep()
if n_attempts_effective < n_avg:
yellowprint("Not enough transforms were collected; try again")
continue
ar_tfm = avg_transform(ar_tfms)
hyd_tfm = avg_transform(hyd_tfms)
all_tfms.append(nlg.inv(ar_tfm).dot(hyd_tfm))
i += 1
return avg_transform(all_tfms)
def extract_snapshots_on_cloud(demo_type, core_type):
"""
runs snapshot extraction on the cloud and saves the result on local machine.
"""
demo_testing_dir = osp.join(testing_results_dir, demo_type)
env_state_files = find_recursive(demo_testing_dir, '*.cp')
state_infos = []
for env_state_file in env_state_files[0:2]:
with open(env_state_file,"r") as fh:
seg_info = cp.load(fh)['seg_info']
if seg_info == None:
continue
save_dir = osp.join(osp.dirname(env_state_file), 'snapshots', osp.splitext(osp.basename(env_state_file))[0])
state_infos.append((seg_info, save_dir))
print colorize("calling on cloud..", "yellow", True)
jids = cloud.map(get_state_snapshots, state_infos, _env='RSS3', _type=core_type)
res = cloud.result(jids)
print colorize("got snapshots from cloud for : %s. Saving..."%demo_type, "green", True)
save_snapshots_from_cloud(res)
def process_observation(self, n_avg=5):
"""
Store an observation of ar_marker and hydras.
"""
raw_input(colorize("Press return when ready to capture transforms.", "green", True))
calib_avg_tfm = []
hydra_avg_tfm = []
j = 0
thresh = n_avg*2
sleeper = rospy.Rate(30)
while j < n_avg:
print colorize('\tGetting averaging transform : %d of %d ...'%(j+1,n_avg), "blue", True)
calib_tfm = self.calib_func()
hydra_tfm = self.get_hydra_transform()
if calib_tfm is None or hydra_tfm is None:
if calib_tfm is None:
yellowprint('Could not find all required AR transforms.')
else:
yellowprint('Could not find all required hydra transforms.')
thresh -= 1
if thresh == 0:
redprint('Tried too many times but could not get enough transforms.')
return False
continue
calib_avg_tfm.append(calib_tfm)
hydra_avg_tfm.append(hydra_tfm)
j += 1
sleeper.sleep()
self.calib_transforms.append(utils.avg_transform(calib_avg_tfm))
self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
return True
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))
sleeper = rospy.Rate(30)
avg_tfms = {}
j = 0
thresh = n_avg*2
pot_avg = 0.0
while j < n_avg:
blueprint('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1))
tfms = {}
# F**k the frames bullshit
ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers)
hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
pot_angle = gmt.get_pot_angle()
if not ar_tfm or (not hyd_tfm and self.hydras):
yellowprint('Could not find all required transforms.')
thresh -= 1
if thresh == 0: return False
continue
pot_avg += pot_angle
j += 1
tfms.update(ar_tfm)
tfms.update(hyd_tfm)
# The angle relevant for each finger is only half the angle.
for marker in tfms:
if marker not in avg_tfms:
avg_tfms[marker] = []
avg_tfms[marker].append(tfms[marker])
sleeper.sleep()
pot_avg /= n_avg
greenprint("Average angle found: %f"%pot_avg)
for marker in avg_tfms:
avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])
update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
return True
def process_observation(self, n_avg=5):
"""
Get an observation and update transform list.
"""
yellowprint("Please hold still for a few seconds.")
self.observation_info = {i:[] for i in xrange(self.num_cameras)}
self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}
# Get RGBD observations
for i in xrange(n_avg):
print colorize("Transform %d out of %d for averaging."%(i,n_avg),'yellow',False)
data = self.cameras.get_RGBD()
for j,cam_data in data.items():
self.observation_info[j].append(cam_data)
# Find AR transforms from RGBD observations and average out transforms.
for i in self.observation_info:
for obs in self.observation_info[i]:
ar_pos = gmt.get_ar_marker_poses (obs['rgb'], obs['depth'])
for marker in ar_pos:
if self.observed_ar_transforms[i].get(marker) is None:
self.observed_ar_transforms[i][marker] = []
self.observed_ar_transforms[i][marker].append(ar_pos[marker])
for marker in self.observed_ar_transforms[i]:
self.observed_ar_transforms[i][marker] = utils.avg_transform(self.observed_ar_transforms[i][marker])
for i in xrange(1,self.num_cameras):
transform = self.find_transform_between_cameras_from_obs(0, i)
if transform is None:
redprint("Did not find a transform between cameras 0 and %d"%i)
continue
if self.transform_list.get(0,i) is None:
self.transform_list[0,i] = []
self.transform_list[0,i].append(transform)
def calibrate (self, n_obs=10, n_avg=5, tfm_pub=None):
if self.num_cameras == 1:
redprint ("Only one camera. You don't need to calibrate.", True)
self.calibrated = True
self.cameras.set_calibrated(True)
return
self.initialize_calibration()
for i in range(n_obs):
yellowprint ("Transform %d out of %d."%(i,n_obs))
if tfm_pub is not None: tfm_pub.set_publish_pc(True)
raw_input(colorize("Press return when you're ready to take the next observation from the cameras.",'green',True))
if tfm_pub is not None: tfm_pub.set_publish_pc(False)
self.process_observation(n_avg)
self.calibrated = self.finish_calibration()
self.cameras.set_calibrated(self.calibrated)
def calibrate_gripper_lite(lr):
global cameras, tfm_pub
greenprint('Step 3.1 Calibrate %s gripper.'%lr)
gr = gripper_lite.GripperLite(lr, marker=gripper_marker_id[lr],
cameras=cameras,
trans_marker_tooltip=gripper_trans_marker_tooltip[lr])
tfm_pub.add_gripper(gr)
tfm_pub.set_publish_grippers(True)
lr_long = {'l':'left','r':'right'}[lr]
if yes_or_no(colorize('Do you want to add hydra to %sgripper?'%lr, 'yellow')):
gr.add_hydra(hydra_marker=lr_long, tfm=None,
ntfm=GRIPPER_LITE_N_OBS,
navg=GRIPPER_LITE_N_AVG)
def get_obs_new_marker(self, marker, n_tfm=10, n_avg=10):
"""
Store a bunch of readings seen for new marker being added.
Returns a list of dicts each of which has marker transforms found
and potentiometer angles.
"""
all_obs = []
i = 0
thresh = n_avg * 2
sleeper = rospy.Rate(30)
while i < n_tfm:
raw_input(colorize("Getting transform %i out of %i. Hit return when ready." % (i, n_tfm), 'yellow', True))
j = 0
j_th = 0
pot_angle = 0
found = True
avg_tfms = []
while j < n_avg:
blueprint("Averaging transform %i out of %i." % (j, n_avg))
ar_tfms = self.cameras.get_ar_markers();
hyd_tfms = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=None);
curr_angle = gmt.get_pot_angle(self.lr)
if not ar_tfms and not hyd_tfms:
yellowprint("Could not find any transform.")
j_th += 1
if j_th < thresh:
continue
else:
found = False
break
tfms = ar_tfms
pot_angle += curr_angle
tfms.update(hyd_tfms)
avg_tfms.append(tfms)
j += 1
sleeper.sleep()
if found is False:
yellowprint("Something went wrong; try again.")
continue
tfms_found = {}
for tfms in avg_tfms:
for m in tfms:
if m not in tfms_found:
tfms_found[m] = []
tfms_found[m].append(tfms[m])
if marker not in tfms_found:
yellowprint("Could not find marker to be added; try again.")
continue
for m in tfms_found:
tfms_found[m] = utils.avg_transform(tfms_found[m])
pot_angle = pot_angle / n_avg
all_obs.append({'tfms':tfms_found, 'pot_angle':pot_angle})
i += 1
return all_obs
def initialize_simple (n_avg=30):
"""
Initializes the file to load from for simple feedback.
Creates AABB out of specified points.
"""
if rospy.get_name() == '/unnamed':
rospy.init_node('simple_feedback_init', anonymous=True)
sleeper = rospy.Rate(30)
yellowprint("You will be asked to move gripper around the feasibility zone.")
lr = raw_input(colorize("Please enter l/r based on which gripper you are using for initialization: ",'yellow',True))
lr_long = {'l':'left', 'r':'right'}[lr]
aabb_points = []
raw_input(colorize("Place the gripper at bottom-left of reachable region closer to you.",'blue',True))
attempts = 10
while attempts > 0:
try:
blc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
print blc_tfm
aabb_points.append(blc_tfm[0:3,3])
break
except Exception as e:
print e
attempts -= 1
yellowprint("Failed attempt. Trying %i more times."%attempts)
sleeper.sleep()
if attempts == 0:
redprint("Could not find hydra.")
raw_input(colorize("Place the gripper at bottom-right of reachable region closer to you.",'blue',True))
attempts = 10
while attempts > 0:
try:
brc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
print brc_tfm[0:3,3]
aabb_points.append(brc_tfm[0:3,3])
break
except:
attempts -= 1
yellowprint("Failed attempt. Trying %i more times."%attempts)
sleeper.sleep()
if attempts == 0:
redprint("Could not find hydra.")
raw_input(colorize("Place the gripper at top-left of reachable region closer to you.",'blue',True))
attempts = 10
while attempts > 0:
try:
tlc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
print tlc_tfm[0:3,3]
aabb_points.append(tlc_tfm[0:3,3])
break
except:
attempts -= 1
yellowprint("Failed attempt. Trying %i more times."%attempts)
sleeper.sleep()
if attempts == 0:
redprint("Could not find hydra.")
raw_input(colorize("Place the gripper at top-right of reachable region closer to you.",'blue',True))
attempts = 10
while attempts > 0:
try:
trc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
print trc_tfm[0:3,3]
aabb_points.append(trc_tfm[0:3,3])
break
except:
attempts -= 1
yellowprint("Failed attempt. Trying %i more times."%attempts)
sleeper.sleep()
if attempts == 0:
redprint("Could not find hydra.")
raw_input(colorize("Place the gripper as far from you as possible in the reachable region.",'blue',True))
attempts = 10
while attempts > 0:
try:
far_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
print far_tfm[0:3,3]
aabb_points.append(far_tfm[0:3,3])
break
except:
attempts -= 1
yellowprint("Failed attempt. Trying %i more times."%attempts)
sleeper.sleep()
if attempts == 0:
redprint("Could not find hydra.")
buffer = 0.05 # a little buffer for reachability
aabb_points = np.asarray(aabb_points)
mins = aabb_points.min(axis=0) - buffer
maxes = aabb_points.max(axis=0) + buffer
aabb_data = {'mins':mins, 'maxes':maxes}
yellowprint("Saving AABB in file...")
aabb_file = osp.join(feedback_dir, simple_feedback_name)
with open(aabb_file,'w') as fh: cPickle.dump(aabb_data, fh)
yellowprint("Saved!")
def initialize_ik (calib_file, n_tfm=5, n_avg=30):
"""
Initializes the file to load from for ik feedback.
First calibrates between PR2 and workspace camera.
Finally ends up with a calibration from the PR2 base with the hydra_base.
Can use hydra_info to reach tool tip from movement data.
"""
if rospy.get_name() == '/unnamed':
rospy.init_node('ik_feedback_init')
cameras = RosCameras()
tfm_pub = CalibratedTransformPublisher(cameras)
yellowprint("Start up the overhead camera as camera1 and PR2's camera as camera2.")
raw_input(colorize("Hit enter when the PR2 is ready at the workstation.",'yellow',True))
c1_frame = 'camera1_link'
h_frame = 'hydra_base'
# Find tfm from bf to camera1_link
greenprint("Calibrating cameras.")
cam_calib = CameraCalibrator(cameras)
done = False
while not done:
cam_calib.calibrate(n_obs=n_tfm, n_avg=n_avg)
if not cam_calib.calibrated:
redprint("Camera calibration failed.")
cam_calib.reset_calibration()
else:
# Add camera to camera tfm and camera to PR2 tfm
tfm_cam = cam_calib.get_transforms()[0]
tfm_pr2 = {'parent':'base_footprint', 'child':c1_frame}
tfm_bf_c2l = get_transform('base_footprint','camera_link')
tfm_pr2['tfm'] = tfm_bf_c2l.dot(np.linalg.inv(tfm_cam['tfm']))
# tfm_pub.add_transforms(cam_calib.get_transforms())
tfm_pub.add_transforms([tfm_cam, tfm_pr2])
print tfm_pub.transforms
if yes_or_no("Are you happy with the calibration? Check RVIZ."):
done = True
tfm_pub.save_calibration(cam_pr2_calib_name)
greenprint("Cameras calibrated.")
else:
yellowprint("Calibrating cameras again.")
cam_calib.reset_calibration()
tfm_c1_h = None
# Now, find transform between base_footprint and hydra base
with open(calib_file,'r') as fh: calib_data = cPickle.load(fh)
for tfm in calib_data['transforms']:
if tfm['parent'] == c1_frame or tfm['parent'] == '/' + c1_frame:
if tfm['child'] == h_frame or tfm['child'] == '/' + h_frame:
tfm_c1_h = tfm['tfm']
if tfm_c1_h is None:
redprint("Hydra calib info not found in %s."%calib_file)
grippers = calib_data['grippers']
assert 'l' in grippers.keys() and 'r' in grippers.keys(), 'Calibration does not have both grippers.'
gprs = {}
for lr,gdata in 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'])
gprs[lr] = gr
ik_file_data = {}
ik_file_data['pr2'] = tfm_pr2['tfm'].dot(tfm_c1_h)
lr_long = {'l':'left','r':'right'}
for lr in gprs:
ik_file_data[lr_long[lr]] = gprs[lr].get_rel_transform(lr_long[lr], 'tool_tip')
yellowprint("Saving IK feedback data in file...")
ik_file = osp.join(feedback_dir, ik_feedback_name)
with open(ik_file,'w') as fh: cPickle.dump(ik_file_data, fh)
yellowprint("Saved!")
def record_demo (demo_name, use_voice):
"""
Record a single demo.
Returns True/False depending on whether demo needs to be saved.
@demo_dir: directory where demo is recorded.
@use_voice: bool on whether to use voice for demo or not.
"""
global cmd_checker, topic_writer, demo_type_dir
# Start here. Change to voice command.
sleeper = rospy.Rate(30)
demo_dir = osp.join(demo_type_dir, demo_name)
started_video = {cam:False for cam in camera_types}
print
yellowprint("Starting bag file recording.")
bag_file = osp.join(demo_dir,demo_names.bag_name)
topic_writer.start_saving(bag_file)
started_bag = True
for cam in camera_types:
yellowprint("Calling saveImagecamera%i service."%cam)
save_image_services[cam](cam_save_requests[cam])
started_video[cam] = True
# Change to voice command
time.sleep(1.2)
subprocess.call("espeak -v en 'Recording.'", stdout=devnull, stderr=devnull, shell=True)
time_start = time.time()
if use_voice:
while True:
status = cmd_checker.get_latest_msg()
if status in ["cancel recording","finish recording","check demo"]:
break
sleeper.sleep()
else:
raw_input(colorize("Press any key when done.",'y',True))
time_finish = time.time()
for cam in started_video:
if started_video[cam]:
save_image_services[cam](cam_stop_request)
yellowprint("Stopped video%i."%cam)
if started_bag:
topic_writer.stop_saving()
yellowprint("Stopped bag.")
if use_voice:
while status == "check demo":
subprocess.call("espeak -v en 'Visualizing.'", stdout=devnull, stderr=devnull, shell=True)
view_hydra_demo_on_rviz(demo_type, demo_name, freq, speed, prompt=False, verbose=False)
time.sleep(0.5)
status = cmd_checker.get_latest_msg()
greenprint("Time taken to record demo: %02f s"%(time_finish-time_start))
return status == "finish recording"
elif yes_or_no("Save demo?"):
greenprint("Time taken to record demo: %02f s"%(time_finish-time_start))
return True
else:
return False
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))
sleeper = rospy.Rate(30)
avg_tfms = {}
pot_avg = 0.0
j = 0
n_attempts_max = n_avg*2
n_attempts_effective = 0;
while j < n_attempts_max:
j += 1
tfms = {}
# F**k the frames bullshit
ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers, camera=1)
hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
pot_angle = gmt.get_pot_angle(self.lr)
if not ar_tfm or (not hyd_tfm and self.hydras):
if not ar_tfm:
yellowprint('Could not find required ar markers from '+str(self.ar_markers))
else:
yellowprint('Could not find required hydra transforms from '+str(self.hydras))
continue
pot_avg += pot_angle
tfms.update(ar_tfm)
tfms.update(hyd_tfm)
blueprint('\tGetting averaging transform: %d of %d ...'%(n_attempts_effective, n_avg-1))
n_attempts_effective += 1
# The angle relevant for each finger is only half the angle.
for marker in tfms:
if marker not in avg_tfms:
avg_tfms[marker] = []
avg_tfms[marker].append(tfms[marker])
if n_attempts_effective == n_avg:
break
sleeper.sleep()
if n_attempts_effective < n_avg:
return False;
pot_avg /= n_avg
greenprint("Average angle found: %f"%pot_avg)
for marker in avg_tfms:
avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])
## only one marker on gripper now
#if len(avg_tfms) == 1:
# yellowprint('Found %i marker only. Not enough to update.'%avg_tfms.keys()[0])
update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
return True
