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 compute_relative_transforms (masterGraph, init=None):
"""
Takes in a transform graph @G such that it has enough data to begin calibration (is_ready(G) returns true).
Optimizes and computes final relative transforms between all nodes (markers).
Returns a graph final_G with the all edges (clique) and final transforms stored in edges.
Make sure the graph is ready before this by calling is_ready(graph,min_obs).
"""
new_mG = nx.DiGraph()
graph_map = {}
for group in masterGraph.nodes_iter():
G = masterGraph.node[group]["graph"]
for i,j in G.edges_iter():
G[i][j]['avg_tfm'] = utils.avg_transform(G[i][j]['transform_list'])
# Optimize rigid body transforms.
graph_map[G] = optimize_transforms(G)
new_mG.add_node(group)
new_mG.node[group]["graph"] = graph_map[G]
if masterGraph.node[group].get("master_marker"):
new_mG.node[group]["master_marker"] = masterGraph.node[group].get("master_marker")
new_mG.node[group]["angle_scale"] = masterGraph.node[group]["angle_scale"]
new_mG.node[group]["markers"] = masterGraph.node[group]["markers"]
new_mG.node[group]["hydras"] = masterGraph.node[group]["hydras"]
new_mG.node[group]["ar_markers"] = masterGraph.node[group]["ar_markers"]
masterGraph.node[group]["primary"] = masterGraph.node[group]["graph"].nodes()[0]
new_mG.node[group]["primary"] = masterGraph.node[group]["primary"]
for g1,g2 in masterGraph.edges_iter():
mg1 = graph_map[masterGraph.node[g1]["graph"]]
mg2 = graph_map[masterGraph.node[g2]["graph"]]
new_mG.add_edge(g1, g2)
node1 = new_mG.node[g1].get("primary")
node2 = new_mG.node[g2].get("primary")
new_mG[g1][g2]['avg_tfm'] = {}
for angle, tfm_data in masterGraph[g1][g2]['transform_list'].items():
transforms = []
# Converting all transforms to standard transform between two fixed nodes.
for tfm in tfm_data:
transforms.append(mg1.edge[node1][tfm['from']]['tfm'].dot(tfm['tfm'])\
.dot(mg2.edge[tfm['to']][node2]['tfm']))
new_mG[g1][g2]['avg_tfm'][angle] = utils.avg_transform(transforms)
# Optimize over angles to get master graph with transforms.
mG_opt = optimize_master_transforms(new_mG, init)
return mG_opt
def get_all_transforms(self, marker_tfms, theta, parent_frame):
"""
From marker transforms given in parent_frame, get all transforms
of all markers on gripper.
"""
ret_tfms = []
cor_avg_tfms = []
for m,tfm in marker_tfms.items():
if m in self.allmarkers:
cor_avg_tfms.append(tfm.dot(self.get_rel_transform(m,'cor', theta)))
if len(cor_avg_tfms) == 0: return ret_tfms
cor_tfm = utils.avg_transform(cor_avg_tfms)
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_%s'%(self.lr, 'cor'),
'tfm':cor_tfm})
for m in self.allmarkers:
if m != 'cor':
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.tt_calculated:
tfm = self.get_rel_transform('cor', 'tool_tip', theta)
ret_tfms.append({'parent':parent_frame,
'child':'%sgripper_%s'%(self.lr, 'tool_tip'),
'tfm':cor_tfm.dot(tfm)})
return ret_tfms
def get_tool_tip_transform(self, marker_tfms, theta):
"""
Get tool tip transfrom from dict of visible markers to transforms
Also provide the angle of gripper (half of what is seen by pot)
Don't need it if only master transform is visible.
m has to be on one of the 'master,'l' or 'r' groups
"""
if self.tt_calculated is False:
redprint ("Tool tip transform not calibrated.")
return
masterg = self.transform_graph.node['master']['graph']
avg_tfms = []
for m in marker_tfms:
if m in self.mmarkers:
tt_tfm = masterg.edge[m]['tool_tip']['tfm']
elif m in self.lmarkers:
tt_tfm = self.transform_graph.edge['l']['master']['tfm_func'](m,'tool_tip', theta)
elif m in self.rmarkers:
tt_tfm = self.transform_graph.edge['r']['master']['tfm_func'](m,'tool_tip', theta)
else:
redprint('Marker not on gripper.')
return
avg_tfms.append(marker_tfms[m].dot(tt_tfm))
return utils.avg_transform(avg_tfms)
def get_markers_transform (self, markers, marker_tfms, theta):
"""
Takes in marker_tfms found, angles and markers for
which transforms are required.
Returns a dict of marker to transforms for the markers
requested, in the same frame as the marker tfms received.
"""
rtn_tfms = {}
cor_avg_tfms = []
for m, tfm in marker_tfms.items():
if m in self.allmarkers:
cor_avg_tfms.append(tfm.dot(self.get_rel_transform(m, 'cor', theta)))
cor_tfm = utils.avg_transform(cor_avg_tfms)
for marker in markers:
if marker in marker_tfms:
rtn_tfms[marker] = marker_tfms[marker]
continue
tfm = self.get_rel_transform('cor', marker, theta)
if tfm is not None:
rtn_tfms[marker] = cor_tfm.dot(tfm)
return rtn_tfms
def get_ar_markers_ros (cameras, parent_frame = None, cams = None, markers=None):
"""
Returns all the ar_markers in the frame of camera 0.
This would potentially take a while.
Do this after all collecting data from everywhere else.
cams -> list of cameras to use
"""
if parent_frame is None:
parent_frame = cameras.parent_frame
ar_markers = {}
data = cameras.get_RGBD(cams)
for cam in data:
tfm = cameras.get_transform_frame(cam, parent_frame)
ar_cam = get_ar_marker_poses(data[cam]['rgb'], data[cam]['depth'])
for marker in ar_cam:
if ar_markers.get(marker) is None:
ar_markers[marker] = []
ar_markers[marker].append(tfm.dot(ar_cam[marker]))
for marker in ar_markers:
ar_markers[marker] = utils.avg_transform(ar_markers[marker])
if markers is None:
return ar_markers
else:
return {marker:ar_markers[marker] for marker in ar_markers if marker in markers}
def get_tooltip_transform(self, marker_tfms, theta):
"""
Get tool tip transform from dict of relevant markers to transforms
Also provide the angle of gripper.
Don't need it if only master transform is visible.
m has to be on one of the 'master,'l' or 'r' groups
"""
if self.tooltip_calculated is False:
redprint ("Tool tip transform not calibrated.")
return
masterg = self.transform_graph.node['master']['graph']
avg_tfms = []
for m in marker_tfms:
if m in self.mmarkers:
tt_tfm = masterg.edge[m]['tool_tip']['tfm']
elif m in self.lmarkers:
tt_tfm = self.transform_graph.edge['l']['master']['tfm_func'].get_tfm(m, 'tool_tip', theta)
elif m in self.rmarkers:
tt_tfm = self.transform_graph.edge['r']['master']['tfm_func'].get_tfm(m, 'tool_tip', theta)
else:
redprint('Marker %s not on gripper.' % m)
continue
avg_tfms.append(marker_tfms[m].dot(tt_tfm))
if len(avg_tfms) == 0:
redprint('No markers on gripper found.')
return
return utils.avg_transform(avg_tfms)
def process_observation_ar(self, n_avg):
"""
Get an observation and update transform list.
n_avg is the number of observations used to compute a transform
"""
# {ar markers for camera i}
self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}
sleeper = rospy.Rate(30)
for i in xrange(n_avg):
for cam_id in xrange(self.num_cameras):
greenprint("Averaging %d out of %d for camera %i" % (i + 1, n_avg, cam_id + 1), False)
tfms = self.cameras.get_ar_markers(camera=cam_id)
for marker in tfms:
if marker not in self.observed_ar_transforms[cam_id]:
self.observed_ar_transforms[cam_id][marker] = []
self.observed_ar_transforms[cam_id][marker].append(tfms[marker])
sleeper.sleep()
for cam_id in self.observed_ar_transforms:
for marker in self.observed_ar_transforms[cam_id]:
self.observed_ar_transforms[cam_id][marker] = utils.avg_transform(self.observed_ar_transforms[cam_id][marker])
observation_updated = False
for i in xrange(1, self.num_cameras):
result = self.extend_camera_pointsets_ar(0, i)
if result is False:
redprint("Did not get info for cameras 0 and %d" % i)
continue
observation_updated = True
return observation_updated
def finish_calibration(self, calib_type='camera'):
"""
Finds the final transform between parent_frame and hydra_base.
"""
if not self.calib_transforms or not self.hydra_transforms: return False
if len(self.calib_transforms) != len(self.hydra_transforms): return False
Tas = solve4(self.calib_transforms, self.hydra_transforms)
avg_hydra_base_transforms = [calib_tfm.dot(Tas).dot(np.linalg.inv(hydra_tfm)) for calib_tfm, hydra_tfm in zip(self.calib_transforms, self.hydra_transforms)]
tfm = {}
tfm['tfm'] = utils.avg_transform(avg_hydra_base_transforms)
if calib_type is 'camera':
pf = self.parent_frame[calib_type]
cname = pf.split('_')[0]
tfm['parent'] = cname+'_link'
tfm['tfm'] = tfm_link_rof.dot(tfm['tfm'])
else:
tfm['parent'] = self.parent_frame[calib_type]
tfm['child'] = 'hydra_base'
self.transforms[calib_type] = tfm
return True
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 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_ar_markers (self, markers=None, camera=None, parent_frame=False, get_time=False):
"""
@markers is a list of markers to be found. Default of None means all markers.
@camera specifies which camera to use. Default of None means all cameras are used
(assuming calibrated).
@parent_frame specifies whether the transforms are in the parent frame or camera frame
(assuming calibrated).
UGLY FUNCTION -- BREAK IT INTO BETTER FUNCTIONS.
"""
if camera is None:
if not self.calibrated:
redprint('Cameras not calibrated. Cannot get transforms from all cameras.')
return {}
marker_tfms = {}
time_stamp = 0.0
num_seen = 0
for i in range(self.num_cameras):
ctfm = self.get_camera_transform(0, i)
tfms, t = self.camera_markers[i].get_marker_transforms(markers, get_time=True)
if tfms:
time_stamp += t
num_seen += 1
for marker in tfms:
if marker not in marker_tfms:
marker_tfms[marker] = []
marker_tfms[marker].append(ctfm.dot(tfms[marker]))
for marker in marker_tfms:
marker_tfms[marker] = utils.avg_transform(marker_tfms[marker])
if num_seen > 0:
time_stamp = time_stamp / num_seen
else:
assert camera in range(self.num_cameras)
marker_tfms, time_stamp = self.camera_markers[camera].get_marker_transforms(markers, get_time=True)
if parent_frame is True:
if not self.calibrated:
redprint('Cameras not calibrated. Cannot get transforms from all cameras.')
return {}
ctfm = self.get_camera_transform(0, camera)
for marker, tfm in marker_tfms.items():
marker_tfms[marker] = ctfm.dot(tfm) # all transforms in reference camera's frame
if get_time:
return marker_tfms, time_stamp
else:
return marker_tfms
def finish_calibration(self):
"""
Finds the final transform between parent_frame and hydra_base.
"""
if not self.ar_transforms or self.hydra_transforms: return False
if len(self.ar_transforms) != len(self.hydra_transforms): return False
Tas = solve4(self.ar_transforms, self.hydra_transforms)
avg_hydra_base_transforms = [ar_tfm.dot(Tas).dot(hydra_tfm) for ar_tfm, hydra_tfm in zip(self.ar_transforms, self.hydra_transforms)]
self.hydra_base_transform = utils.avg_transform(avg_hydra_base_transforms)
self.cameras.stop_streaming()
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))
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 finish_calibration_pycb(self, avg=False):
cb_transforms = {i:[] for i in range(self.num_cameras)}
I = np.eye(4)
for i in self.image_list:
yellowprint("Getting transform data for camera %i." % (i + 1))
ind = 1
for img in self.image_list[i]:
blueprint("... Observation %i." % ind)
ind += 1
tfm = cu.get_checkerboard_transform(img, chessboard_size=chessboard_size)
if tfm is None: return False
cb_transforms[i].append(tfm)
rel_tfms = {i:[] for i in range(1, self.num_cameras)}
for i in range(1, self.num_cameras):
cam_transform = {}
cam_transform['parent'] = 'camera1_link'
cam_transform['child'] = 'camera%d_link' % (i + 1)
for j in range(len(cb_transforms[i])):
rtfm = cb_transforms[0][j].dot(nlg.inv(cb_transforms[i][j]))
rel_tfms[i].append(rtfm)
if avg:
tfm = utils.avg_transform(rel_tfms[i])
# convert from in to cm
tfm[0:3, 3] *= 1.0 # 0.0254
cam_transform['tfm'] = tfm_link_rof.dot(tfm).dot(np.linalg.inv(tfm_link_rof))
else:
scores = []
for j, rtfm in enumerate(rel_tfms[i]):
scores.append(0)
rtfm = rel_tfms[i][j]
for k, rtfm2 in enumerate(rel_tfms[i]):
if j == k:
continue
scores[j] += nlg.norm(rtfm.dot(nlg.inv(rtfm2)) - I)
print "Scores:", scores
tfm = rel_tfms[i][np.argmin(scores)]
# convert from in to m
tfm[0:3, 3] *= 1.0 # 0.0254
cam_transform['tfm'] = tfm_link_rof.dot(tfm).dot(np.linalg.inv(tfm_link_rof))
self.camera_transforms[0, i] = cam_transform
return True
def quick_calibrate(NUM_CAM, N_AVG):
rospy.init_node('quick_calibrate')
cameras = RosCameras(NUM_CAM)
tfm_pub = tf.TransformBroadcaster()
frames = {i:'/camera%i_link'%(i+1) for i in xrange(NUM_CAM)}
calib_tfms = {(frames[0],frames[i]):None for i in xrange(1,NUM_CAM)}
sleeper = rospy.Rate(30)
try:
while True:
tfms_found = {i:{} for i in xrange(NUM_CAM)}
for _ in xrange(N_AVG):
for i in xrange(NUM_CAM):
mtfms = cameras.get_ar_markers(camera=i)
for m in mtfms:
if m not in tfms_found[i]:
tfms_found[i][m] = []
tfms_found[i][m].append(mtfms[m])
for i in tfms_found:
for m in tfms_found[i]:
tfms_found[i][m] = utils.avg_transform(tfms_found[i][m])
for i in xrange(1,NUM_CAM):
rof_tfm = find_transform(tfms_found[0], tfms_found[i])
if rof_tfm is not None:
calib_tfms[(frames[0], frames[i])] = tfm_link_rof.dot(rof_tfm).dot(nlg.inv(tfm_link_rof))
for parent, child in calib_tfms:
if calib_tfms[parent, child] is not None:
trans, rot = conversions.hmat_to_trans_rot(calib_tfms[parent, child])
tfm_pub.sendTransform(trans, rot,
rospy.Time.now(),
child, parent)
sleeper.sleep()
except KeyboardInterrupt:
print "got ctrl-c"
def finish_calibration(self):
"""
Average out transforms and store final values.
Return true/false based on whether transforms were found.
"""
if not self.transform_list: return False
for c1,c2 in self.transform_list:
cam_transform= {}
cam_transform['tfm'] = utils.avg_transform(self.transform_list[key])
cam_transform['parent'] = 'camera%d_depth_optical_frame'%(c1+1)
cam_transform['child'] = 'camera%d_depth_optical_frame'%(c2+1)
self.camera_transforms[c1,c2] = cam_transform
self.cameras.stop_streaming()
self.cameras.store_calibrated_transforms(self.camera_transforms)
return True
def get_avg_hydra_tfm(lr, n_avg, sleeper):
"""
Returns averaged hydra tfms.
"""
if not isinstance(lr, list): lr = [lr]
avg_tfms = {h:[] for h in lr}
j = 0
while j < n_avg:
hydra_tfms = gmt.get_hydra_transforms('hydra_base', lr)
if not hydra_tfms:
continue
for h in hydra_tfms:
avg_tfms[h].append(hydra_tfms[h])
j += 1
sleeper.sleep()
for h in lr: avg_tfms[h] = avg_transform(avg_tfms[h])
return avg_tfms
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 estimate_initial_transform(self):
'''
When using chessboard for calibration, there may be ambiguity due to the symmetric of chessboard.
As a result, AR markers are used to remove ambiguity
'''
raw_input("Place AR marker(s) visible to all cameras to get an 'initial' estimate. Then hit enter.")
N_AVG = 10
self.est_tfms = {cam_id:{} for cam_id in xrange(1, self.num_cameras)}
tfms_found = {cam_id:{} for cam_id in xrange(self.num_cameras)}
sleeper = rospy.Rate(30)
for _ in xrange(N_AVG):
for cam_id in xrange(self.num_cameras):
mtfms = self.cameras.get_ar_markers(camera=cam_id)
for m in mtfms:
if m not in tfms_found[cam_id]:
tfms_found[cam_id][m] = []
tfms_found[cam_id][m].append(mtfms[m])
sleeper.sleep()
for cam_id in tfms_found:
for m in tfms_found[cam_id]:
tfms_found[cam_id][m] = utils.avg_transform(tfms_found[cam_id][m])
for cam_id in xrange(1, self.num_cameras):
ar1, ar2 = common_ar_markers(tfms_found[0], tfms_found[cam_id])
if not ar1 or not ar2:
redprint("No common AR Markers found between camera 1 and %i" % (cam_id + 1))
return False
if len(ar1.keys()) == 1:
self.est_tfms[0, cam_id] = ar1.values()[0].dot(nlg.inv(ar2.values()[0]))
else:
self.est_tfms[0, cam_id] = find_rigid_tfm(convert_hmats_to_points(ar1.values(), True),
convert_hmats_to_points(ar2.values(), True))
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 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_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_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_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 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
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 add_marker (self, marker, group, m_type='AR', n_tfm=10, n_avg=30):
"""
Add marker to the gripper, after calibration.
@marker: marker id (a number for AR markers, either 'left' or 'right' for hydras)
@group: 'l','r' or 'master'
@m_type: type of marker - 'AR' or 'hydra'
@n_tfm,@n_avg - the usual
"""
# Checks to see if data + situation is valid to add marker
if marker in self.allmarkers:
greenprint("Marker already added.")
return
if self.cameras is None:
redprint("Sorry, have no cameras to check.")
return
if group not in ['master', 'l', 'r']:
redprint('Invalid group %s' % group)
return
if m_type not in ['AR', 'hydra']:
redprint('Invalid marker type %s' % m_type)
return
# Get observations
all_obs = self.get_obs_new_marker(marker, n_tfm, n_avg)
# Compute average relative transform between correct primary node and marker
primary_node = self.transform_graph.node[group]['primary']
avg_rel_tfms = []
for obs in all_obs:
tfms = obs['tfms']
angle = obs['pot_angle']
marker_tfm = tfms[marker]
primary_tfm = self.get_markers_transform([primary_node], tfms, angle)[primary_node]
avg_rel_tfms.append(nlg.inv(primary_tfm).dot(marker_tfm))
rel_tfm = utils.avg_transform(avg_rel_tfms)
# Add markers to relevant lists
self.transform_graph.node[group]['markers'].append(marker)
self.allmarkers.append(marker)
if m_type == 'AR':
self.transform_graph.node[group]['ar_markers'].append(marker)
self.ar_markers.append(marker)
else:
self.transform_graph.node[group]['hydras'].append(marker)
self.hydra_markers.append(marker)
if group == 'master':
self.mmarkers.append(marker)
elif group == 'l':
self.lmarkers.append(marker)
else:
self.rmarkers.append(marker)
# Update graphs from transform found
graph = self.transform_graph.node[group]['graph']
graph.add_node(marker)
for node in graph.nodes_iter():
graph.add_edge(node, marker)
graph.add_edge(marker, node)
tfm = graph.edge[node][primary_node]['tfm'].dot(rel_tfm)
graph.edge[node][marker]['tfm'] = tfm
graph.edge[marker][node]['tfm'] = nlg.inv(tfm)
greenprint("Successfully added your marker to the gripper!")
def optimize_master_transforms (mG, init=None):
"""
Optimize transforms over the masterGraph (which is a metagraph with nodes as rigid body graphs).
"""
idx = 1
node_map = {}
master = None
for node in mG.nodes_iter():
if mG.node[node].get("master_marker") is not None:
master = node
node_map[node] = 0
else:
node_map[node] = idx
idx += 1
# Some predefined variables
I3 = np.eye(3)
def get_mat_from_node (X, node, offset = None):
"""
Get the matrix from the objective vector depending on the node.
If offset is specified, return the matrix at the position directly.
"""
if offset is not None:
return X[offset:offset+12].reshape([3,4], order='F')
offset = node_map[node]*12
Xij = X[offset:offset+12]
return Xij.reshape([3,4], order='F')
def f_objective (X):
"""
Objective function to make transforms close to average transforms.
Sum of the norms of matrix differences between each Tij and
"""
obj = 0
zaxis = np.array([0,0,1])
for g1,g2 in mG.edges_iter():
for angle in mG.edge[g1][g2]['avg_tfm']:
t1 = np.r_[get_mat_from_node(X,g1),np.array([[0,0,0,1]])]
t2 = np.r_[get_mat_from_node(X,g2),np.array([[0,0,0,1]])]
rad = utils.rad_angle(angle)
tot_angle = rad*(-mG.node[g1]["angle_scale"]+mG.node[g2]["angle_scale"])
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(zaxis, tot_angle)
tfm = t1.dot(rot).dot(nlg.inv(t2))
obj += nlg.norm(tfm - mG.edge[g1][g2]['avg_tfm'][angle])
return obj
def f_constraints (X):
"""
Constraint function to force matrices to be valid transforms (R.T*R = I_3).
"""
con = []
# Constrain Ris to be valid rotations
for node in node_map:
Ri = get_mat_from_node(X, node)[0:3,0:3]
con.append(nlg.norm(Ri.T.dot(Ri) - I3))
return np.asarray(con)
## Intial value assumes each rigid body has some edge with master.
## Averaging out over angles.
if init is not None:
x_init = init
else:
x_init = np.zeros(12*mG.number_of_nodes())
x_init[0:9] = I3.reshape(9)
for node in mG.neighbors_iter(master):
init_tfm = utils.avg_transform(mG.edge[master][node]['avg_tfm'].values())
print init_tfm.dot(np.r_[np.c_[np.eye(3),np.array([0,0,0])],np.array([[0,0,0,0]])]).dot(init_tfm.T)
offset = node_map[node]*12
x_init[offset:offset+12] = init_tfm[0:3,:].reshape(12, order='F')
##
print "Initial x: ", x_init
print "Initial objective: ", f_objective(x_init)
(X, fx, _, _, _) = sco.fmin_slsqp(func=f_objective, x0=x_init, f_eqcons=f_constraints, iter=200, full_output=1, iprint=2)
mG_opt = nx.DiGraph()
## modify master graph
node0 = mG.node[master]["primary"]
masterG = mG.node[master]["graph"]
node_iter = masterG.neighbors(node0)
masterG.add_node("cor")
# change master primary
mG.node[master]["primary"] = "cor"
masterG.add_edge(node0,"cor")
masterG.add_edge("cor",node0)
masterG.add_edge("cor","cor")
Tsc = np.r_[get_mat_from_node(X, master), np.array([[0,0,0,1]])]
masterG.edge[node0]["cor"]['tfm'] = Tsc
masterG.edge["cor"][node0]['tfm'] = nlg.inv(Tsc)
masterG.edge["cor"]["cor"]['tfm'] = np.eye(4)
for node in node_iter:
masterG.add_edge(node,"cor")
masterG.add_edge("cor",node)
tfm = masterG.edge[node][node0]['tfm'].dot(Tsc)
masterG.edge[node]["cor"]['tfm'] = tfm
masterG.edge["cor"][node]['tfm'] = nlg.inv(tfm)
mG_opt.add_node(master)
mG_opt.node[master]["graph"] = masterG
mG_opt.node[master]["angle_scale"] = 0
mG_opt.node[master]["primary"] = "cor"
mG_opt.node[master]["master_marker"] = mG.node[master]["master_marker"]
mG_opt.node[master]["markers"] = mG.node[master]["markers"]
mG_opt.node[master]["hydras"] = mG.node[master]["hydras"]
mG_opt.node[master]["ar_markers"] = mG.node[master]["ar_markers"]
## add edges to the rest
for group in mG.nodes_iter():
if group == master: continue
mG_opt.add_edge(master, group)
mG_opt.add_edge(group, master)
mG_opt.node[group]["graph"] = mG.node[group]["graph"]
mG_opt.node[group]["angle_scale"] = mG.node[group]["angle_scale"]
mG_opt.node[group]["primary"] = mG.node[group]["primary"]
mG_opt.node[group]["hydras"] = mG.node[group]["hydras"]
mG_opt.node[group]["ar_markers"] = mG.node[group]["ar_markers"]
tfm = np.r_[get_mat_from_node(X, group), np.array([[0,0,0,1]])]
mG_opt.edge[group][master]['tfm'] = tfm
mG_opt.edge[master][group]['tfm'] = nlg.inv(tfm)
tfmFuncs = tfmClass(group, mG_opt, master, masterG)
tfmFuncsInv = tfmClassInv(group, mG_opt, master, masterG)
mG_opt.edge[master][group]['tfm_func'] = tfmFuncs
mG_opt.edge[group][master]['tfm_func'] = tfmFuncsInv
return mG_opt