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 get_all_transforms(self, diff_cam=False):
"""
From marker transforms given in parent_frame, get all transforms
of all markers on gripper.
diff_cam: whether multiple cameras are used.
"""
if self.cameras is None:
redprint("Cameras not initialized. Could not get transforms.")
return
if diff_cam:
ar_tfms = {}
for i in range(self.cameras.num_cameras):
# parent_frame=True, so tfms is the transform from parent_frame (camera1) to ar_marker
tfms = self.cameras.get_ar_markers(camera=i, parent_frame=True, markers=[self.ar_marker])
if tfms: ar_tfms[i] = tfms[self.ar_marker]
else:
# default camera=0, so even parent_frame=False, ar_tfms still store the transform from camera1 to ar_marker
ar_tfms = self.cameras.get_ar_markers(markers=[self.ar_marker])
transforms = []
# return T_camera_tooltip (if diff_cam, to different camera; otherwise to camera0)
if diff_cam:
for i,tfm in ar_tfms.items():
# each tfm is T_camera1_gripper_ar_marker
# get_tooltip_transform(self.ar_marker, tfm) returns T_camerai_tooltip
transforms.append({'parent':self.parent_frame,
'child':'%sgripper_camera%i_tooltip'%(self.lr, i+1),
'tfm':self.get_tooltip_transform(self.ar_marker, tfm)
})
elif ar_tfms:
transforms.append({'parent':self.parent_frame,
'child':'%sgripper_ar_tooltip'%self.lr,
'tfm':self.get_tooltip_transform(self.ar_marker, ar_tfms[self.ar_marker])
})
if self.hydra_marker is not None:
hyd_tfms = gmt.get_hydra_transforms(self.parent_frame, hydras=[self.hydra_marker])
if hyd_tfms:
tfm = hyd_tfms[self.hydra_marker]
transforms.append({'parent':self.parent_frame,
'child':'%sgripper_hydra_tooltip'%self.lr,
'tfm':self.get_tooltip_transform(self.hydra_marker, tfm)
})
return transforms
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):
"""
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_specific_tooltip_tfm (self, m_type='AR'):
"""
Not used in any places
Get the estimate of the tool tip from either ar_markers or hydras.
"""
if m_type not in ['AR', 'hydra']:
redprint('Not sure what estimate %s gives.' % m_type)
return
if m_type == 'AR':
if len(self.ar_markers) == 0:
redprint('No AR markers to give you an estimate.')
return
marker_tfms = self.cameras.get_ar_markers(markers=self.ar_markers)
else:
if len(self.hydra_markers) == 0:
redprint('No hydras to give you an estimate.')
return
marker_tfms = gmt.get_hydra_transforms(self.parent_frame, self.hydra_markers)
theta = gmt.get_pot_angle(self.lr)
return self.get_tooltip_transform(marker_tfms, theta)
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 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 get_all_transforms(self, parent_frame, diff_cam=False):
"""
From marker transforms given in parent_frame, get all transforms
of all markers on gripper.
"""
if self.cameras is None:
redprint("Cameras not initialized. Could not get transforms.")
return
ar_tfms = self.cameras.get_ar_markers()
if diff_cam:
ar_tfms_cam = {}
for i in range(self.cameras.num_cameras):
ar_tfms_cam[i] = self.cameras.get_ar_markers(camera=i, parent_frame=True)
hyd_tfms = gmt.get_hydra_transforms(self.parent_frame, None)
theta = gmt.get_pot_angle(self.lr)
marker_tfms = ar_tfms
marker_tfms.update(hyd_tfms)
ret_tfms = []
cor_avg_tfms = []
cor_hyd_avg = []
cor_ar_avg = []
ar_found = False
hyd_found = False
for m, tfm in marker_tfms.items():
if m in self.ar_markers:
c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta))
cor_ar_avg.append(c_tfm)
cor_avg_tfms.append(c_tfm)
ar_found = True
elif m in self.hydra_markers:
c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta))
cor_hyd_avg.append(c_tfm)
cor_avg_tfms.append(c_tfm)
hyd_found = True
if diff_cam:
cor_ar_cams = {}
cam_found = {i:False for i in ar_tfms_cam}
for i in ar_tfms_cam:
for m, tfm in ar_tfms_cam[i].items():
if m in self.ar_markers:
c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta))
if i not in cor_ar_cams:
cor_ar_cams[i] = []
cam_found[i] = True
cor_ar_cams[i].append(c_tfm)
if cam_found[i]:
cor_ar_cams[i] = utils.avg_transform(cor_ar_cams[i])
if len(cor_avg_tfms) == 0: return ret_tfms
cor_tfm = utils.avg_transform(cor_avg_tfms)
cor_h_tfm = utils.avg_transform(cor_hyd_avg)
cor_a_tfm = utils.avg_transform(cor_ar_avg)
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_%s' % (self.lr, 'cor'),
'tfm':cor_tfm})
for m in self.allmarkers:
if m != 'cor' and m != 'tool_tip':
tfm = self.get_rel_transform('cor', m, theta)
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_%s' % (self.lr, m),
'tfm':cor_tfm.dot(tfm)})
if self.tooltip_calculated:
tfm = self.get_rel_transform('cor', 'tool_tip', theta)
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_tooltip' % self.lr,
'tfm':cor_tfm.dot(tfm)})
if hyd_found:
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_tooltip_hydra' % self.lr,
'tfm':cor_h_tfm.dot(tfm)})
if ar_found:
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_tooltip_ar' % self.lr,
'tfm':cor_a_tfm.dot(tfm)})
if diff_cam:
for i, cor_tfm in cor_ar_cams.items():
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_tooltip_camera%i' % (self.lr, i + 1),
'tfm':cor_tfm.dot(tfm)})
return ret_tfms
def get_hydra_transform(self):
tfms = gmt.get_hydra_transforms('hydra_base', [self.calib_hydra])
if tfms is None: return None
return tfms[self.calib_hydra]
