00001
00002 import roslib; roslib.load_manifest('hai_sandbox')
00003 import rospy
00004
00005
00006
00007
00008
00009
00010
00011
00012 from geometry_msgs.msg import PointStamped, Point
00013 import tf.transformations as tr
00014 from std_msgs.msg import String
00015 import visualization_msgs.msg as vm
00016
00017
00018 import tf
00019 import cv
00020 import functools as ft
00021
00022
00023 import numpy as np
00024 import math
00025 import time
00026 import subprocess as sb
00027 import scipy.spatial as sp
00028 import pdb
00029 import os
00030 import os.path as pt
00031 import shutil
00032
00033
00034 import hrl_camera.ros_camera as rc
00035 import hrl_pr2_lib.pr2 as pr2
00036 import hrl_pr2_lib.devices as hd
00037 import hrl_lib.rutils as ru
00038 import hrl_lib.tf_utils as tfu
00039 import hrl_lib.util as ut
00040
00041 import hrl_pr2_lib.linear_move as lm
00042
00043
00044
00045 import hai_sandbox.recognize_3d as r3d
00046 import hrl_lib.image3d as i3d
00047 import hrl_lib.viz as viz
00048 import cProfile
00049 import threading
00050 import dynamic_reconfigure.client as dr
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 class TaskError(Exception):
00061 def __init__(self, value):
00062 self.parameter = value
00063
00064 def __str__(self):
00065 return repr(self.parameter)
00066
00067 class ActionType:
00068 def __init__(self, inputs, outputs):
00069 self.inputs = inputs
00070 self.outputs = outputs
00071
00072 class ParamType:
00073 def __init__(self, name, ptype, options=None):
00074 self.name = name
00075 self.ptype = ptype
00076 self.options = options
00077
00078 class Action:
00079
00080 def __init__(self, name, params):
00081 self.name = name
00082 self.params = params
00083
00084 class BehaviorDescriptor:
00085
00086 def __init__(self):
00087 self.descriptors = {
00088 'twist': ActionType([ParamType('angle', 'radian')], [ParamType('success', 'bool')]),
00089 'linear_move': ActionType([ParamType('start_loc', 'se3'),
00090 ParamType('movement', 'r3'),
00091 ParamType('stop', 'discrete', ['pressure', 'pressure_accel'])],
00092 [ParamType('success', 'bool')]),
00093 }
00094
00095 start_location = (np.matrix([0.3, 0.15, 0.9]).T, np.matrix([0., 0., 0., 0.1]))
00096 movement = np.matrix([.4, 0, 0.]).T
00097 self.seed = [Action('linear_move', [start_location, movement, 'pressure']),
00098 Action('linear_move', [Action('current_location', [])])]
00099 self.run(self.seed)
00100
00101 def run(self, seed):
00102 pass
00103
00104
00105 class LaserPointerClient:
00106 def __init__(self, target_frame='/base_link', tf_listener=None, robot=None):
00107 self.dclick_cbs = []
00108
00109 self.point_cbs = []
00110 self.target_frame = target_frame
00111 self.laser_point_base = None
00112 self.robot = robot
00113 self.base_sound_path = (sb.Popen(["rospack", "find", "hai_sandbox"], stdout=sb.PIPE).communicate()[0]).strip()
00114
00115 if tf_listener == None:
00116 self.tf_listener = tf.TransformListener()
00117 else:
00118 self.tf_listener = tf_listener
00119
00120 rospy.Subscriber('cursor3d', PointStamped, self.laser_point_handler)
00121 self.double_click = rospy.Subscriber('mouse_left_double_click', String, self.double_click_cb)
00122 self.robot.sound.waveSound(self.base_sound_path + '/sounds/beep.wav').play()
00123
00124 def transform_point(self, point_stamped):
00125 point_head = point_stamped.point
00126
00127 base_T_head = tfu.transform(self.target_frame, point_stamped.header.frame_id, self.tf_listener)
00128 point_mat_head = tfu.translation_matrix([point_head.x, point_head.y, point_head.z])
00129 point_mat_base = base_T_head * point_mat_head
00130 t_base, _ = tfu.matrix_as_tf(point_mat_base)
00131 return np.matrix(t_base).T
00132
00133 def laser_point_handler(self, point_stamped):
00134 self.robot.sound.waveSound(self.base_sound_path + '/sounds/blow.wav').play()
00135 self.laser_point_base = self.transform_point(point_stamped)
00136 for f in self.point_cbs:
00137 f(self.laser_point_base)
00138
00139 def double_click_cb(self, a_str):
00140 rospy.loginfo('Double CLICKED')
00141 self.robot.sound.waveSound(self.base_sound_path + '/sounds/beep.wav').play()
00142
00143 for f in self.dclick_cbs:
00144 f(self.laser_point_base)
00145 self.laser_point_base = None
00146
00147
00148 def add_double_click_cb(self, func):
00149 self.dclick_cbs.append(func)
00150
00151
00152
00153
00154 def add_point_cb(self, func):
00155 self.point_cbs.append(func)
00156
00157
00158 def image_diff_val2(before_frame, after_frame):
00159 br = np.asarray(before_frame)
00160 ar = np.asarray(after_frame)
00161 max_sum = br.shape[0] * br.shape[1] * br.shape[2] * 255.
00162 sdiff = np.abs((np.sum(br) / max_sum) - (np.sum(ar) / max_sum))
00163
00164 return sdiff
00165
00166
00167 class ManipulationBehaviors:
00168
00169 def __init__(self, arm, pr2_obj, tf_listener=None):
00170 try:
00171 rospy.init_node('linear_move', anonymous=True)
00172 except Exception, e:
00173 rospy.loginfo('call to init_node failed %s' % str(e))
00174 self.movement = lm.LinearReactiveMovement(arm, pr2_obj, tf_listener)
00175
00176
00177
00178
00179 def reach(self, point, pressure_thres,\
00180 reach_direction=np.matrix([0,0,0]).T, orientation=None):
00181 MOVEMENT_TOLERANCE = .1
00182
00183
00184
00185
00186 self.movement.set_pressure_threshold(pressure_thres)
00187 loc_bl = self.movement.arm_obj.pose_cartesian_tf()[0]
00188 front_loc = point.copy()
00189 front_loc[0,0] = max(loc_bl[0,0], .4)
00190
00191
00192
00193 if orientation == None:
00194 start_loc = self.movement.arm_obj.pose_cartesian_tf()
00195 orientation = start_loc[1]
00196 self.movement.pressure_listener.rezero()
00197
00198
00199 r1, residual_error = self.movement.move_absolute((front_loc, orientation), stop='pressure', pressure=pressure_thres)
00200
00201
00202
00203
00204
00205
00206
00207 pos_error = None
00208 try:
00209
00210
00211
00212
00213 point_reach = point + reach_direction
00214 r2, pos_error = self.movement.move_absolute((point_reach, \
00215 self.movement.arm_obj.pose_cartesian_tf()[1]), stop='pressure_accel', pressure=pressure_thres)
00216
00217 except lm.RobotSafetyError, e:
00218 rospy.loginfo('robot safety error %s' % str(e))
00219 r2 = None
00220
00221 touch_loc_bl = self.movement.arm_obj.pose_cartesian_tf()
00222
00223 if r2 == 'pressure' or r2 == 'accel' or (pos_error != None and (pos_error < (MOVEMENT_TOLERANCE + np.linalg.norm(reach_direction)))):
00224 self.movement.pressure_listener.rezero()
00225
00226
00227
00228
00229 cur_pose, cur_ang = self.movement.arm_obj.pose_cartesian_tf()
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 return True, r2, touch_loc_bl
00244 else:
00245
00246
00247 return False, r2, None
00248
00249 def press(self, direction, press_pressure, contact_pressure):
00250
00251 self.movement.set_movement_mode_cart()
00252
00253 r1, diff_1 = self.movement.move_relative_gripper(direction, stop='pressure', pressure=contact_pressure)
00254
00255 if r1 == 'pressure' or r1 == 'accel':
00256 self.movement.set_movement_mode_cart()
00257 r2, diff_2 = self.movement.move_relative_gripper(direction, stop='pressure_accel', pressure=press_pressure)
00258 if r2 == 'pressure' or r2 == 'accel' or r2 == None:
00259 return True, r2
00260 else:
00261 return False, r2
00262 else:
00263 return False, r1
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290 def separate_by_labels(points, labels):
00291 pidx = np.where(labels == r3d.POSITIVE)[1].A1.tolist()
00292 nidx = np.where(labels == r3d.NEGATIVE)[1].A1.tolist()
00293 uidx = np.where(labels == r3d.UNLABELED)[1].A1.tolist()
00294 return points[:, uidx], points[:, pidx], points[:, nidx]
00295
00296
00297 class LocationDisplay(threading.Thread):
00298
00299 def __init__(self, loc_man):
00300 threading.Thread.__init__(self)
00301 try:
00302 rospy.init_node('location_display')
00303 except Exception,e:
00304 print e
00305
00306 self.location_label_pub = rospy.Publisher('location_label', vm.Marker)
00307 self.location_marker_pub = rospy.Publisher('location_marker', vm.Marker)
00308 self.loc_man = loc_man
00309
00310 def run(self):
00311 text_scale = .1
00312 text_color = np.matrix([1,0,0,1.]).T
00313
00314 circle_radii = .9
00315 circle_scale = .2
00316 circle_z = .03
00317 circle_color = np.matrix([1,0,0,1.]).T
00318
00319 circle_msgs = []
00320 data = self.loc_man.data
00321 pdb.set_trace()
00322 for task_id in data.keys():
00323 point_map = data[task_id]['center']
00324 circle_msgs.append(viz.circle_marker(point_map, circle_radii, circle_scale, circle_color, 'map', circle_z))
00325
00326 text_msgs = []
00327 for task_id in data.keys():
00328 point_map = data[task_id]['center']
00329 text_msgs.append(viz.text_marker(task_id, point_map, text_color, text_scale, 'map'))
00330
00331 r = rospy.Rate(2)
00332 while not rospy.is_shutdown():
00333 for c in circle_msgs:
00334 self.location_marker_pub.publish(c)
00335 for txt in text_msgs:
00336 self.location_label_pub.publish(txt)
00337 r.sleep()
00338
00339
00340
00341 class LocationManager:
00342
00343 def __init__(self, name, rec_params):
00344 self.RELIABILITY_RECORD_LIM = 20
00345 self.RELIABILITY_THRES = .9
00346
00347 self.rec_params = rec_params
00348 self.saved_locations_fname = name
00349 self.LOCATION_ADD_RADIUS = .5
00350 self.tree = None
00351 self.centers = None
00352
00353 self.ids = []
00354 self.data = {}
00355
00356 self.learners = {}
00357 self._load_database()
00358
00359
00360 self.image_pubs = {}
00361
00362
00363
00364
00365
00366 for k in self.data.keys():
00367 self.train(k)
00368 self.image_pubs[k] = r3d.ImagePublisher(k.replace(':', '_'))
00369
00370 self.task_types = ['light_switch_down', 'light_switch_up',
00371 'light_rocker_down', 'light_rocker_up',
00372 'push_drawer', 'pull_drawer']
00373 self.task_pairs = [['light_switch_down', 'light_switch_up'],
00374 ['light_rocker_down', 'light_rocker_up'],
00375 ['pull_drawer', 'push_drawer']]
00376 self.driving_param = {'light_switch_up': {'coarse': .9, 'fine': .6, 'voi': .4},
00377 'light_switch_down': {'coarse': .9, 'fine': .6, 'voi': .4},
00378
00379 'light_rocker_down': {'coarse': .9, 'fine': .6, 'voi': .4},
00380 'light_rocker_up': {'coarse': .9, 'fine': .6, 'voi': .4},
00381
00382 'pull_drawer': {'coarse': .9, 'fine': .5, 'voi': .4},
00383 'push_drawer': {'coarse': .9, 'fine': .5, 'voi': .4}}
00384
00385 def revert(self):
00386 self.centers = self.centers[:, 0:4]
00387 self.ids = self.ids[0:4]
00388 self.data.pop('office_push_drawer')
00389 self.data.pop('office_pull_drawer')
00390
00391
00392 def _load_database(self):
00393
00394
00395 if not os.path.isfile(self.saved_locations_fname):
00396 return
00397 d = ut.load_pickle(self.saved_locations_fname)
00398 self.ids = d['ids']
00399 self.centers = d['centers']
00400 self.data = d['data']
00401 self.tree = sp.KDTree(np.array(self.centers).T)
00402
00403 def save_database(self):
00404 print 'Saving pickle. DONOT INTERRUPPT!!!'
00405 d = {'centers': self.centers,
00406 'ids': self.ids,
00407 'data': self.data}
00408 try:
00409 shutil.copyfile(self.saved_locations_fname,
00410 time.strftime('%m_%d_%Y_%I_%M%p') + '_locations.pkl')
00411 except Exception, e:
00412 print e
00413
00414 ut.save_pickle(d, self.saved_locations_fname)
00415 print 'SAFE!!!'
00416
00417
00418 def get_complementary_task(self, tasktype):
00419 for ta, tb in self.task_pairs:
00420 if ta == tasktype:
00421 return tb
00422 if tb == tasktype:
00423 return ta
00424 return None
00425
00426 def update_base_pose(self, taskid, base_pose):
00427 print 'updating base pose for task', taskid
00428 self.data[taskid]['base_pose'] = base_pose
00429
00430 def create_new_location(self, task_type, point_map, base_pose, gather_data=True, name=None):
00431 if name == None:
00432 taskid = time.strftime('%A_%m_%d_%Y_%I:%M%p') + ('_%s' % task_type)
00433 else:
00434 taskid = name + ('_%s' % task_type)
00435
00436 try:
00437 os.mkdir(taskid)
00438 except OSError, e:
00439 print e
00440
00441 if self.centers == None:
00442 self.centers = point_map
00443 else:
00444 self.centers = np.column_stack((self.centers, point_map))
00445 self.tree = sp.KDTree(np.array(self.centers.T))
00446
00447 self.ids.append(taskid)
00448 self.data[taskid] = {'task': task_type,
00449 'center': point_map,
00450 'base_pose': base_pose,
00451 'points': point_map,
00452 'dataset': None,
00453 'dataset_raw': None,
00454 'gather_data': gather_data,
00455 'complementary_task_id': None,
00456 'pca': None,
00457 'execution_record': []}
00458 self.image_pubs[taskid] = r3d.ImagePublisher(taskid.replace(':', '_'))
00459 self.save_database()
00460 return taskid
00461
00462 def record_time(self, task_id, record_name, value):
00463 if not self.data[task_id].has_key('times'):
00464 self.data[task_id]['times'] = {}
00465
00466 if not self.data[task_id]['times'].has_key(record_name):
00467 self.data[task_id]['times'][record_name] = []
00468
00469 self.data[task_id]['times'][record_name].append(value)
00470
00471 def update_execution_record(self, taskid, value):
00472 self.data[taskid]['execution_record'].append(value)
00473
00474 def is_reliable(self, taskid):
00475 record = self.data[taskid]['execution_record']
00476 if len(record) < self.RELIABILITY_RECORD_LIM:
00477 return False
00478
00479 if np.sum(record) < (self.RELIABILITY_RECORD_LIM * self.RELIABILITY_THRES):
00480 return False
00481
00482 return True
00483
00484 def _id_to_center_idx(self, task_id):
00485 for i, tid in enumerate(self.ids):
00486 if tid == task_id:
00487 return i
00488 return None
00489
00490 def add_perceptual_data(self, task_id, fea_dict):
00491 rospy.loginfo('LocationManager: add_perceptual_data - %s adding %d instance(s)' \
00492 % (task_id, fea_dict['labels'].shape[1]))
00493 current_raw_dataset = self.data[task_id]['dataset_raw']
00494 current_dataset = self.data[task_id]['dataset']
00495
00496 self.data[task_id]['dataset_raw'] = \
00497 r3d.InterestPointDataset.add_to_dataset(
00498 current_raw_dataset, fea_dict['instances'],
00499 fea_dict['labels'], fea_dict['points2d'],
00500 fea_dict['points3d'], None, None,
00501 sizes=fea_dict['sizes'])
00502
00503 self.data[task_id]['dataset'] = \
00504 r3d.InterestPointDataset.add_to_dataset(
00505 current_dataset, fea_dict['instances'],
00506 fea_dict['labels'], fea_dict['points2d'],
00507 fea_dict['points3d'], None, None,
00508 sizes=fea_dict['sizes'])
00509
00510 def get_perceptual_data(self, task_id):
00511 return self.data[task_id]['dataset']
00512
00513 def remove_perceptual_data(self, task_id, instance_idx):
00514 self.data[task_id]['dataset'].remove(instance_idx)
00515 self.data[task_id]['dataset_raw'].remove(instance_idx)
00516
00517 def active_learn_add_data(self, task_id, fea_dict):
00518
00519 self.add_perceptual_data(task_id, fea_dict)
00520
00521 def update(self, task_id, point_map):
00522
00523 ldata = self.data[task_id]
00524 ldata['points'] = np.column_stack((point_map, ldata['points']))
00525 ldata['center'] = ldata['points'].mean(1)
00526
00527 center_idx = self._id_to_center_idx(task_id)
00528 self.centers[:, center_idx] = ldata['center']
00529 self.tree = sp.KDTree(np.array(self.centers).T)
00530
00531 def set_center(self, task_id, point_map):
00532 ldata = self.data[task_id]
00533 ldata['points'] = point_map
00534 ldata['center'] = point_map
00535 center_idx = self._id_to_center_idx(task_id)
00536 self.centers[:, center_idx] = ldata['center']
00537 self.tree = sp.KDTree(np.array(self.centers).T)
00538
00539 def publish_image(self, task_id, image, postfix=''):
00540 self.image_pubs[task_id].publish(image)
00541 ffull = pt.join(task_id, time.strftime('%A_%m_%d_%Y_%I_%M_%S%p') + postfix + '.jpg')
00542 cv.SaveImage(ffull, image)
00543
00544 def list_all(self):
00545 rlist = []
00546 for k in self.data.keys():
00547 rlist.append([k, self.data[k]['task']])
00548 return rlist
00549
00550 def list_close_by(self, point_map, task=None):
00551 if self.tree != None:
00552 indices = self.tree.query_ball_point(np.array(point_map.T), self.LOCATION_ADD_RADIUS)[0]
00553 print 'list_close_by: indices close by', indices
00554
00555 ids_selected = []
00556 for i in indices:
00557 sid = self.ids[i]
00558 stask = self.data[sid]['task']
00559 if task == None:
00560 ids_selected.append([sid, stask])
00561 else:
00562 if task == stask:
00563 ids_selected.append([sid, stask])
00564 return ids_selected
00565 else:
00566 return []
00567
00568 def train_all_classifiers(self):
00569 for k in self.data.keys():
00570 self.train(k)
00571
00572 def train(self, task_id, dset_for_pca=None, save_pca_images=True):
00573 dataset = self.data[task_id]['dataset']
00574 rec_params = self.rec_params
00575
00576 if dataset == None:
00577 return
00578
00579
00580 nneg = np.sum(dataset.outputs == r3d.NEGATIVE)
00581 npos = np.sum(dataset.outputs == r3d.POSITIVE)
00582 print '================= Training ================='
00583 print 'NEG examples', nneg
00584 print 'POS examples', npos
00585 print 'TOTAL', dataset.outputs.shape[1]
00586 neg_to_pos_ratio = float(nneg)/float(npos)
00587 weight_balance = ' -w0 1 -w1 %.2f' % neg_to_pos_ratio
00588 print 'training'
00589
00590 previous_learner = None
00591 if self.learners.has_key(task_id):
00592 previous_learner = self.learners[task_id]
00593
00594 learner = r3d.SVMPCA_ActiveLearner(use_pca=True,
00595 reconstruction_std_lim=self.rec_params.reconstruction_std_lim,
00596 reconstruction_err_toler=self.rec_params.reconstruction_err_toler,
00597 old_learner=previous_learner, pca=self.data[task_id]['pca'])
00598
00599
00600 if dset_for_pca != None:
00601 inputs_for_pca = dset_for_pca['instances']
00602 else:
00603
00604 inputs_for_pca = dataset.inputs
00605
00606 learner.train(dataset,
00607 inputs_for_pca,
00608 rec_params.svm_params + weight_balance,
00609 rec_params.variance_keep)
00610
00611 self.data[task_id]['pca'] = learner.pca
00612 self.learners[task_id] = learner
00613 if save_pca_images:
00614
00615 basis = learner.pca.projection_basis
00616 cv.SaveImage('%s_pca.png' % task_id, r3d.instances_to_image(self.rec_params.win_size, basis, np.min(basis), np.max(basis)))
00617
00618
00619
00620
00621
00622 class ApplicationBehaviors:
00623
00624 def __init__(self):
00625
00626 rospy.init_node('linear_move', anonymous=True)
00627 self.tf_listener = tf.TransformListener()
00628 self.robot = pr2.PR2(self.tf_listener, base=True)
00629 self.behaviors = ManipulationBehaviors('l', self.robot, tf_listener=self.tf_listener)
00630 self.laser_scan = hd.LaserScanner('point_cloud_srv')
00631
00632 self.prosilica = rc.Prosilica('prosilica', 'polled')
00633 self.prosilica_cal = rc.ROSCameraCalibration('/prosilica/camera_info')
00634
00635 self.wide_angle_camera_left = rc.ROSCamera('/wide_stereo/left/image_rect_color')
00636 self.wide_angle_configure = dr.Client('wide_stereo_both')
00637
00638 self.laser_listener = LaserPointerClient(tf_listener=self.tf_listener, robot=self.robot)
00639 self.laser_listener.add_double_click_cb(self.click_cb)
00640 self.rec_params = r3d.Recognize3DParam()
00641
00642 self.OPTICAL_FRAME = 'high_def_optical_frame'
00643 self.feature_ex = r3d.NarrowTextureFeatureExtractor(self.prosilica,
00644 hd.PointCloudReceiver('narrow_stereo_textured/points'),
00645 self.prosilica_cal,
00646 self.robot.projector,
00647 self.tf_listener, self.rec_params)
00648
00649
00650
00651
00652 self.critical_error = False
00653
00654 self.start_location_light_switch = (np.matrix([0.35, 0.30, 1.1]).T, np.matrix([0., 0., 0., 0.1]))
00655 self.start_location_drawer = (np.matrix([0.20, 0.40, .8]).T,
00656 np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
00657 self.folded_pose = np.matrix([ 0.10134791, -0.29295995, 0.41193769]).T
00658 self.create_arm_poses()
00659 self.learners = {}
00660
00661
00662
00663
00664
00665
00666
00667 self.driving_posture('light_switch_down')
00668 self.robot.projector.set(False)
00669 self.locations_man = LocationManager('locations_narrow_v11.pkl', rec_params=self.rec_params)
00670
00671 def draw_dots_nstuff(self, img, points2d, labels, picked_loc):
00672 pidx = np.where(labels == r3d.POSITIVE)[1].A1.tolist()
00673 nidx = np.where(labels == r3d.NEGATIVE)[1].A1.tolist()
00674 uidx = np.where(labels == r3d.UNLABELED)[1].A1.tolist()
00675
00676 if picked_loc != None:
00677 r3d.draw_points(img, picked_loc, [255, 0, 0], 4, -1)
00678
00679
00680 if len(uidx) > 0:
00681 upoints = points2d[:, uidx]
00682 r3d.draw_points(img, upoints, [255,255,255], 2, -1)
00683
00684 if len(nidx) > 0:
00685 npoints = points2d[:, nidx]
00686 r3d.draw_points(img, npoints, [0,0,255], 2, -1)
00687
00688 if len(pidx) > 0:
00689 ppoints = points2d[:, pidx]
00690 r3d.draw_points(img, ppoints, [0,255,0], 2, -1)
00691
00692 def create_arm_poses(self):
00693 self.right_tucked = np.matrix([[-0.02362532, 1.10477102, -1.55669475, \
00694 -2.12282706, -1.41751231, -1.84175899, 0.21436806]]).T
00695
00696 self.left_tucked = np.matrix([[ 0.05971848, 1.24980184, 1.79045674, \
00697 -1.68333801, -1.73430635, -0.09838841, -0.08641928]]).T
00698
00699
00700 self.r0 = np.matrix([[-0.22774141, 0.7735819 , -1.45102092, \
00701 -2.12152412, -1.14684579, -1.84850287, 0.21397648]]).T
00702
00703
00704 self.l0 = np.matrix([[ 0.06021592, 1.24844832, 1.78901355, -1.68333801, 1.2, -0.10152105, -0.08641928]]).T
00705
00706
00707 self.l1 = np.matrix([[0.94524406, 1.24726399, 1.78548574, -1.79148173, 1.20027637, -1.0, -0.08633226]]).T
00708
00709
00710 self.l2 = np.matrix([[ 1.53180837, 1.24362641, 1.78452361, -1.78829678, 1.1996979,-1.00446167, -0.08741998]]).T
00711
00712 def untuck(self):
00713
00714 if np.linalg.norm(self.robot.left.pose() - self.left_tucked) < .3:
00715 rospy.loginfo('untuck: not in tucked position. Ignoring request')
00716 return
00717
00718 self.behaviors.movement.set_movement_mode_ik()
00719 self.robot.right.set_pose(self.r0, 1.)
00720 self.robot.left.set_poses(np.column_stack([self.l0, self.l1, self.l2]), \
00721 np.array([1., 2., 3.]))
00722 self.robot.right.set_pose(self.right_tucked, 1.)
00723 self.behaviors.movement.set_movement_mode_cart()
00724
00725 def tuck(self):
00726
00727 if np.linalg.norm(self.robot.left.pose() - self.left_tucked) < .5:
00728 rospy.loginfo('tuck: Already tucked. Ignoring request.')
00729 return
00730
00731 self.behaviors.movement.set_movement_mode_ik()
00732 self.robot.right.set_pose(self.r0, 1.)
00733 self.robot.left.set_poses(np.column_stack([self.l2, self.l1, self.l0, self.left_tucked]), \
00734 np.array([4., 5., 6., 7.]))
00735 self.robot.right.set_pose(self.right_tucked, 1.)
00736 self.behaviors.movement.set_movement_mode_cart()
00737
00738
00739
00740
00741
00742
00743
00744 def camera_change_detect(self, threshold, f, args):
00745 config = self.wide_angle_configure.get_configuration()
00746 config['auto_gain'] = False
00747 config['auto_exposure'] = False
00748 self.wide_angle_configure.update_configuration(config)
00749
00750
00751 start_pose = self.robot.head.pose()
00752
00753
00754 self.robot.head.set_pose(np.radians(np.matrix([30., -20]).T), 1)
00755 time.sleep(4)
00756 for i in range(7):
00757 before_frame = self.wide_angle_camera_left.get_frame()
00758 cv.SaveImage('before.png', before_frame)
00759 f_return = f(*args)
00760 time.sleep(5)
00761 for i in range(7):
00762 after_frame = self.wide_angle_camera_left.get_frame()
00763
00764 cv.SaveImage('after.png', after_frame)
00765 sdiff = image_diff_val2(before_frame, after_frame)
00766 self.robot.head.set_pose(start_pose, 1)
00767 self.robot.head.set_pose(start_pose, 1)
00768 time.sleep(3)
00769
00770
00771 config['auto_gain'] = True
00772 config['auto_exposure'] = True
00773 self.wide_angle_configure.update_configuration(config)
00774
00775 rospy.loginfo('camera difference %.4f (thres %.3f)' % (sdiff, threshold))
00776 if sdiff > threshold:
00777 rospy.loginfo('difference detected!')
00778 return True, f_return
00779 else:
00780 rospy.loginfo('NO differences detected!')
00781 return False, f_return
00782
00783 def close_gripper(self):
00784 GRIPPER_CLOSE = .003
00785
00786 self.behaviors.movement.gripper_close()
00787
00788 def open_gripper(self):
00789 GRIPPER_OPEN = .08
00790
00791 self.behaviors.movement.gripper_open()
00792
00793 def light_switch1(self, point,
00794 point_offset, press_contact_pressure,
00795 press_pressure, press_distance, visual_change_thres):
00796
00797 try:
00798
00799
00800
00801 rospy.loginfo('reaching to ' + str(point.T))
00802
00803
00804 self.close_gripper()
00805
00806 time.sleep(1)
00807 self.behaviors.movement.pressure_listener.rezero()
00808
00809
00810
00811
00812
00813 success, reason, touchloc_bl = self.behaviors.reach(point, \
00814 press_contact_pressure, \
00815 reach_direction=np.matrix([0.1,0,0]).T)
00816
00817
00818
00819 if touchloc_bl != None:
00820 dist = np.linalg.norm(point - touchloc_bl[0])
00821
00822
00823
00824 rospy.loginfo('Touched point is %.3f m away from observed point' % dist)
00825
00826
00827
00828 if not success:
00829 error_msg = 'Reach failed due to "%s"' % reason
00830 rospy.loginfo(error_msg)
00831 rospy.loginfo('Failure recovery: moving back')
00832 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='accel', \
00833 pressure=press_contact_pressure)
00834
00835 return False, None, point+point_offset
00836
00837 rospy.loginfo('pressing')
00838
00839
00840 self.behaviors.movement.pressure_listener.rezero()
00841 change, press_ret = self.camera_change_detect(visual_change_thres, \
00842 self.behaviors.press, \
00843 (press_distance, press_pressure, press_contact_pressure))
00844 success, reason = press_ret
00845 if not success:
00846 rospy.loginfo('Press failed due to "%s"' % reason)
00847
00848
00849
00850 rospy.loginfo('moving back')
00851
00852 r1, pos_error1 = self.behaviors.movement.move_relative_gripper(np.matrix([-.03, 0., 0.]).T, \
00853 stop='none', pressure=press_contact_pressure)
00854 if r1 != None:
00855 rospy.loginfo('moving back failed due to "%s"' % r1)
00856 return change, None, point+point_offset
00857
00858 rospy.loginfo('reseting')
00859 self.behaviors.movement.pressure_listener.rezero()
00860 r2, pos_error2 = self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00861 if r2 != None and r2 != 'no solution':
00862 rospy.loginfo('moving back to start location failed due to "%s"' % r2)
00863 return change, None, point+point_offset
00864 self.behaviors.movement.pressure_listener.rezero()
00865
00866 rospy.loginfo('DONE.')
00867 return change, touchloc_bl, point+point_offset
00868
00869 except lm.RobotSafetyError, e:
00870 rospy.loginfo('>>>> ROBOT SAFETY ERROR! RESETTING. %s' % str(e))
00871 self.behaviors.movement.pressure_listener.rezero()
00872 r2, pos_error2 = self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00873 return change, None, point+point_offset
00874
00875
00876 def light_rocker_push(self, point, pressure, visual_change_thres, offset):
00877 rospy.loginfo('Reaching')
00878 linear_movement = self.behaviors.movement
00879
00880 self.close_gripper()
00881 self.behaviors.movement.pressure_listener.rezero()
00882
00883
00884 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00885
00886
00887
00888 def reach_with_back_up(point, thres, reach_direction):
00889 self.behaviors.reach(point, thres, reach_direction)
00890 try:
00891 r1, pos_error1 = self.behaviors.movement.move_relative_gripper(np.matrix([-.05, 0., 0.]).T, stop='none')
00892 except lm.RobotSafetyError, e:
00893 rospy.loginfo('robot safety error %s' % str(e))
00894 change, press_ret = self.camera_change_detect(visual_change_thres, \
00895
00896 reach_with_back_up, \
00897
00898 (point, pressure, np.matrix([.1,0,0]).T))
00899 try:
00900 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel')
00901 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00902 except lm.RobotSafetyError, e:
00903 rospy.loginfo('robot safety error %s' % str(e))
00904
00905 try:
00906 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00907 except lm.RobotSafetyError, e:
00908 rospy.loginfo('robot safety error %s' % str(e))
00909
00910 rospy.loginfo('Reseting')
00911 return change, '', point+offset
00912
00913
00914 def drawer_push(self, point_bl):
00915 PUSH_TOLERANCE = .1
00916
00917 linear_movement = self.behaviors.movement
00918
00919
00920 self.open_gripper()
00921 self.behaviors.movement.pressure_listener.rezero()
00922
00923 rospy.loginfo("Moving to start location")
00924
00925
00926 linear_movement.move_absolute((self.start_location_drawer[0],
00927
00928 np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))),
00929 stop='pressure_accel', pressure=1000)
00930
00931
00932 self.behaviors.movement.set_pressure_threshold(1000)
00933 loc_bl = self.behaviors.movement.arm_obj.pose_cartesian_tf()[0]
00934 front_loc = point_bl.copy()
00935 front_loc[0,0] = max(loc_bl[0,0], .4)
00936
00937
00938
00939 rospy.loginfo("Moving to front location")
00940
00941 orientation = np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))
00942 self.behaviors.movement.pressure_listener.rezero()
00943 r1, residual_error = self.behaviors.movement.move_absolute((front_loc, orientation),
00944 stop='pressure', pressure=1500)
00945 linear_movement.pressure_listener.rezero()
00946
00947
00948 rospy.loginfo("Touching surface")
00949 try:
00950 linear_movement.move_relative_gripper(np.matrix([.5,0,0]).T, stop='pressure_accel', pressure=100)
00951 except lm.RobotSafetyError, e:
00952 rospy.loginfo('robot safety error %s' % str(e))
00953 contact_loc_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
00954
00955
00956 rospy.loginfo("PUSH!!!")
00957 current_position = self.robot.left.pose_cartesian_tf()
00958 target_position = current_position[0] + np.matrix([.4,0,0.]).T
00959 try:
00960
00961 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00962 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00963 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00964 except lm.RobotSafetyError, e:
00965 rospy.loginfo('robot safety error %s' % str(e))
00966
00967 pushed_loc_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
00968
00969 rospy.loginfo("Moving away")
00970 try:
00971 linear_movement.move_relative_gripper(np.matrix([-.05,0,0]).T, stop='accel')
00972 except lm.RobotSafetyError, e:
00973 rospy.loginfo('robot safety error %s' % str(e))
00974 try:
00975 linear_movement.move_relative_gripper(np.matrix([-.10,0,0]).T, stop='accel')
00976 except lm.RobotSafetyError, e:
00977 rospy.loginfo('robot safety error %s' % str(e))
00978 try:
00979 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel')
00980 except lm.RobotSafetyError, e:
00981 rospy.loginfo('robot safety error %s' % str(e))
00982
00983 linear_movement.pressure_listener.rezero()
00984
00985 linear_movement.move_absolute((self.start_location_drawer[0],
00986
00987 np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))),
00988 stop='pressure_accel', pressure=1000)
00989
00990 move_dist = np.linalg.norm(contact_loc_bl - pushed_loc_bl)
00991 rospy.loginfo('pushed for distance %.3f' % move_dist)
00992 success = move_dist > PUSH_TOLERANCE
00993 return success, 'pushed', pushed_loc_bl
00994
00995
00996 def drawer(self, point):
00997
00998 GRIPPER_OPEN = .08
00999 GRIPPER_CLOSE = .003
01000 MAX_HANDLE_SIZE = .03
01001 linear_movement = self.behaviors.movement
01002 gripper = self.robot.left_gripper
01003
01004
01005
01006 self.open_gripper()
01007 linear_movement.move_absolute((self.start_location_drawer[0],
01008
01009 np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0))),
01010 stop='pressure_accel', pressure=1000)
01011
01012
01013 success, reason, touchloc_bl = self.behaviors.reach(point, 300,
01014 reach_direction=np.matrix([0.1, 0, 0]).T,
01015 orientation=np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
01016
01017
01018 if not success:
01019 error_msg = 'Reach failed due to "%s"' % reason
01020 rospy.loginfo(error_msg)
01021 rospy.loginfo('Failure recovery: moving back')
01022 try:
01023 linear_movement.move_relative_gripper(np.matrix([-.25,0,0]).T, stop='pressure_accel', pressure=300)
01024 except lm.RobotSafetyError, e:
01025 rospy.loginfo('robot safety error %s' % str(e))
01026 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='accel', pressure=300)
01027 return False, 'reach failed', point
01028
01029
01030 GRASP_THRES = 100
01031 try:
01032 linear_movement.move_relative_gripper(np.matrix([-.01,0,0]).T, stop='none')
01033 except lm.RobotSafetyError, e:
01034 rospy.loginfo('robot safety error %s' % str(e))
01035
01036
01037 self.close_gripper()
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052 grasped_handle = (gripper.pose()[0,0] > GRIPPER_CLOSE) and (gripper.pose()[0,0] < MAX_HANDLE_SIZE)
01053
01054 if not grasped_handle:
01055 rospy.loginfo('Failed to grasp handle :(')
01056
01057 self.open_gripper()
01058
01059 linear_movement.pressure_listener.rezero()
01060 linear_movement.move_relative_gripper(np.matrix([-.25,0,0]).T, stop='pressure_accel', pressure=300)
01061 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='accel', pressure=300)
01062 return False, 'failed to grasp handle', point
01063
01064
01065 linear_movement.pressure_listener.rezero()
01066 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel', pressure=2500)
01067 linear_movement.move_absolute(linear_movement.arm_obj.pose_cartesian_tf(),
01068 stop='pressure_accel', pressure=300)
01069
01070
01071 self.close_gripper()
01072 rospy.sleep(1)
01073 linear_movement.pressure_listener.rezero()
01074
01075
01076
01077 still_has_handle = gripper.pose()[0,0] > GRIPPER_CLOSE
01078
01079 try:
01080 linear_movement.move_relative_base(np.matrix([-.15,0,0]).T, stop='accel', pressure=2500)
01081 except lm.RobotSafetyError, e:
01082
01083 self.open_gripper()
01084 linear_movement.pressure_listener.rezero()
01085 rospy.loginfo('robot safety error %s' % str(e))
01086
01087
01088
01089 location_handle_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
01090
01091
01092 self.open_gripper()
01093 rospy.sleep(2)
01094 linear_movement.pressure_listener.rezero()
01095
01096
01097 linear_movement.move_relative_base(np.matrix([-.2, 0, 0.]).T, stop='pressure_accel', pressure=1000)
01098 linear_movement.move_relative_base(np.matrix([-.1, .2, 0.1]).T, stop='pressure_accel', pressure=1000)
01099 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='pressure_accel', pressure=1000)
01100
01101 return still_has_handle, 'pulled', location_handle_bl
01102
01103
01104
01105
01106 def drive_approach_behavior(self, point_bl, dist_far):
01107
01108
01109 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
01110 point_map = tfu.transform_points(map_T_base_link, point_bl)
01111 t_current_map, r_current_map = self.robot.base.get_pose()
01112 rospy.loginfo('drive_approach_behavior: point is %.3f m away"' % np.linalg.norm(t_current_map[0:2].T - point_map[0:2,0].T))
01113
01114 point_dist = np.linalg.norm(point_bl)
01115 bounded_dist = np.max(point_dist - dist_far, 0)
01116 point_close_bl = (point_bl / point_dist) * bounded_dist
01117 point_close_map = tfu.transform_points(map_T_base_link, point_close_bl)
01118 rvalue = self.robot.base.set_pose(point_close_map.T.A1.tolist(), \
01119 r_current_map, '/map', block=True)
01120 t_end, r_end = self.robot.base.get_pose()
01121 rospy.loginfo('drive_approach_behavior: ended up %.3f m away from laser point' % np.linalg.norm(t_end[0:2] - point_map[0:2,0].T))
01122 rospy.loginfo('drive_approach_behavior: ended up %.3f m away from goal' % np.linalg.norm(t_end[0:2] - point_close_map[0:2,0].T))
01123 rospy.loginfo('drive_approach_behavior: returned %d' % rvalue)
01124 return rvalue
01125
01126
01127
01128
01129 def approach_perpendicular_to_surface(self, point_bl, voi_radius, dist_approach):
01130
01131
01132
01133
01134
01135 map_T_base_link0 = tfu.transform('map', 'base_link', self.tf_listener)
01136 point_map0 = tfu.transform_points(map_T_base_link0, point_bl)
01137
01138 self.turn_to_point(point_bl, block=False)
01139
01140 point_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), \
01141 point_map0)
01142 point_cloud_bl = self.laser_scan.scan(math.radians(180.), math.radians(-180.), 2.5)
01143 point_cloud_np_bl = ru.pointcloud_to_np(point_cloud_bl)
01144 rospy.loginfo('approach_perpendicular_to_surface: pointcloud size %d' \
01145 % point_cloud_np_bl.shape[1])
01146 voi_points_bl, limits_bl = i3d.select_rect(point_bl, voi_radius, voi_radius, voi_radius, point_cloud_np_bl)
01147
01148 normal_bl = i3d.calc_normal(voi_points_bl)
01149 point_in_front_mechanism_bl = point_bl + normal_bl * dist_approach
01150 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
01151 point_in_front_mechanism_map = tfu.transform_points(map_T_base_link, point_in_front_mechanism_bl)
01152
01153
01154 point_map = tfu.transform_points(map_T_base_link, point_bl)
01155 t_current_map, r_current_map = self.robot.base.get_pose()
01156 rospy.loginfo('approach_perpendicular_to_surface: driving for %.3f m to front of surface' \
01157 % np.linalg.norm(t_current_map[0:2] - point_in_front_mechanism_map[0:2,0].T))
01158
01159 rvalue = self.robot.base.set_pose(point_in_front_mechanism_map.T.A1.tolist(), r_current_map, 'map')
01160 if rvalue != 3:
01161 return rvalue
01162
01163 t1_current_map, r1_current_map = self.robot.base.get_pose()
01164 rospy.loginfo('approach_perpendicular_to_surface: %.3f m away from from of surface' % np.linalg.norm(t1_current_map[0:2] - point_in_front_mechanism_map[0:2,0].T))
01165
01166
01167 base_link_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01168 point_bl = tfu.transform_points(base_link_T_map, point_map)
01169
01170 self.turn_to_point(point_bl, block=False)
01171 time.sleep(2.)
01172
01173 return rvalue
01174
01175
01176
01177
01178 def approach_location(self, point_bl, coarse_stop, fine_stop, voi_radius=.2):
01179
01180 point_dist = np.linalg.norm(point_bl[0:2,0])
01181 rospy.loginfo('approach_location: Point is %.3f away.' % point_dist)
01182 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
01183 point_map = tfu.transform_points(map_T_base_link, point_bl)
01184
01185 dist_theshold = coarse_stop + .1
01186 if point_dist > dist_theshold:
01187 rospy.loginfo('approach_location: Point is greater than %.1f m away (%.3f). Driving closer.' % (dist_theshold, point_dist))
01188 rospy.loginfo('approach_location: point_bl ' + str(point_bl.T))
01189
01190 ret = self.drive_approach_behavior(point_bl, dist_far=coarse_stop)
01191 base_link_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01192 point_bl_t1 = tfu.transform_points(base_link_T_map, point_map)
01193 if ret != 3:
01194 dist_end = np.linalg.norm(point_bl_t1[0:2,0])
01195 if dist_end > dist_theshold:
01196 rospy.logerr('approach_location: drive_approach_behavior failed! %.3f' % dist_end)
01197 self.robot.sound.say("I am unable to navigate to that location")
01198 return False, 'failed'
01199
01200 ret = self.approach_perpendicular_to_surface(point_bl_t1, voi_radius=voi_radius, dist_approach=fine_stop)
01201 if ret != 3:
01202 rospy.logerr('approach_location: approach_perpendicular_to_surface failed!')
01203 return False, 'failed'
01204
01205 self.robot.sound.say('done')
01206 rospy.loginfo('approach_location: DONE DRIVING!')
01207 return True, 'done'
01208 else:
01209 return False, 'ignored'
01210
01211 def turn_to_point(self, point_bl, block=True):
01212 ang = math.atan2(point_bl[1,0], point_bl[0,0])
01213 rospy.loginfo('turn_to_point: turning by %.2f deg' % math.degrees(ang))
01214
01215 self.robot.base.turn_by(-ang, block=block, overturn=True)
01216
01217
01218
01219
01220
01221 def stationary_light_switch_behavior(self, point_bl):
01222 while True:
01223 print 'Enter a command u(ntuck), s(tart), l(ight), t(uck), e(x)it.'
01224 a = raw_input()
01225
01226 if a == 'u':
01227 self.untuck()
01228
01229 if a == 's':
01230 self.behaviors.movement.pressure_listener.rezero()
01231 self.behaviors.movement.set_movement_mode_cart()
01232 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
01233
01234
01235
01236 if a == 'l':
01237 point_offset = np.matrix([0, 0, 0.03]).T
01238 success, _ = self.light_switch1(point_bl, point_offset=point_offset, \
01239 press_contact_pressure=300, \
01240 press_pressure=3500, press_distance=np.matrix([0,0,-.15]).T, \
01241 visual_change_thres=.03)
01242
01243
01244 if a == 't':
01245 self.tuck()
01246
01247 if a == 'x':
01248 break
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273
01274
01275
01276
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317
01318
01319
01320
01321
01322
01323
01324
01325
01326
01327
01328
01329
01330
01331
01332
01333
01334
01335
01336
01337
01338
01339
01340
01341
01342
01343
01344
01345
01346
01347
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361
01362
01363
01364
01365
01366
01367
01368
01369
01370
01371
01372 def location_approach_driving(self, task, point_bl):
01373
01374 ap_result = self.approach_location(point_bl,
01375 coarse_stop=self.locations_man.driving_param[task]['coarse'],
01376 fine_stop=self.locations_man.driving_param[task]['fine'],
01377 voi_radius=self.locations_man.driving_param[task]['voi'])
01378
01379 if ap_result[1] == 'failed':
01380 return False, 'approach_location failed'
01381
01382 if ap_result[1] == 'ignore':
01383
01384 ret = self.approach_perpendicular_to_surface(point_bl,
01385 voi_radius=self.locations_man.driving_param[task]['voi'],
01386 dist_approach=self.locations_man.driving_param[task]['fine'])
01387 if ret != 3:
01388 rospy.logerr('location_approach_driving: approach_perpendicular_to_surface failed!')
01389 return False, 'approach_perpendicular_to_surface failed'
01390 else:
01391 return True, None
01392
01393 return True, None
01394
01395
01396
01397
01398
01399
01400
01401
01402
01403
01404 def get_behavior_by_task(self, task_type):
01405 if task_type == 'light_switch_down':
01406 return ft.partial(self.light_switch1,
01407
01408 point_offset=np.matrix([0,0, -.08]).T,
01409 press_contact_pressure=300,
01410
01411 press_pressure=6000,
01412 press_distance=np.matrix([0.01,0,-.15]).T,
01413 visual_change_thres=.025)
01414
01415 elif task_type == 'light_switch_up':
01416 return ft.partial(self.light_switch1,
01417
01418 point_offset=np.matrix([0,0,.08]).T,
01419 press_contact_pressure=300,
01420
01421 press_pressure=6000,
01422 press_distance=np.matrix([0.01,0,.15]).T,
01423 visual_change_thres=.025)
01424
01425 elif task_type == 'light_rocker_up':
01426 return ft.partial(self.light_rocker_push,
01427 pressure=500,
01428 visual_change_thres=.025, offset=np.matrix([0,0,-.05]).T)
01429
01430 elif task_type == 'light_rocker_down':
01431 return ft.partial(self.light_rocker_push,
01432 pressure=500,
01433 visual_change_thres=.025, offset=np.matrix([0,0,.05]).T)
01434
01435 elif task_type == 'pull_drawer':
01436 return self.drawer
01437
01438 elif task_type == 'push_drawer':
01439 return self.drawer_push
01440
01441 else:
01442 pdb.set_trace()
01443
01444
01445
01446 def manipulation_posture(self, task_type):
01447 self.robot.projector.set(False)
01448 for i in range(3):
01449 self.prosilica.get_frame()
01450 self.robot.projector.set(True)
01451
01452
01453 self.robot.left_gripper.open(False, .005)
01454
01455 self.behaviors.movement.pressure_listener.rezero()
01456
01457 if task_type == 'light_switch_down' or task_type == 'light_switch_up':
01458 if np.linalg.norm(self.start_location_light_switch[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
01459 return
01460 self.robot.torso.set_pose(.2, True)
01461 self.untuck()
01462 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
01463 self.behaviors.movement.pressure_listener.rezero()
01464
01465 elif task_type == 'light_rocker_up' or task_type == 'light_rocker_down':
01466 if np.linalg.norm(self.start_location_light_switch[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
01467 return
01468 self.robot.torso.set_pose(.2, True)
01469 self.untuck()
01470 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
01471 self.behaviors.movement.pressure_listener.rezero()
01472
01473 elif task_type == 'pull_drawer' or task_type == 'push_drawer':
01474 if np.linalg.norm(self.start_location_drawer[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
01475 return
01476 self.robot.torso.set_pose(0.01, True)
01477 self.untuck()
01478 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='pressure')
01479 self.behaviors.movement.pressure_listener.rezero()
01480 else:
01481 pdb.set_trace()
01482
01483
01484 def driving_posture(self, task_type):
01485 self.robot.projector.set(False)
01486 self.close_gripper()
01487
01488 if np.linalg.norm(self.folded_pose - self.robot.left.pose_cartesian_tf()[0]) < .1:
01489 return
01490
01491 self.robot.torso.set_pose(0.03, True)
01492 self.robot.left_gripper.open(False, .005)
01493
01494 self.behaviors.movement.pressure_listener.rezero()
01495
01496 if task_type == 'light_switch_down' or task_type == 'light_switch_up':
01497 self.tuck()
01498
01499 elif task_type == 'light_rocker_up' or task_type == 'light_rocker_down':
01500 self.tuck()
01501
01502 elif task_type == 'pull_drawer' or task_type == 'push_drawer':
01503 self.tuck()
01504 else:
01505 pdb.set_trace()
01506
01507
01508 def look_at(self, point_bl, block=True):
01509
01510
01511
01512
01513
01514 self.robot.head.look_at(point_bl-np.matrix([0,0,.15]).T, pointing_frame=self.OPTICAL_FRAME,
01515 pointing_axis=np.matrix([1,0,0.]).T, wait=block)
01516
01517
01518
01519
01520
01521
01522 def scenario_user_clicked_at_location(self, point_bl):
01523
01524
01525 self.look_at(point_bl, False)
01526 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
01527 point_map = tfu.transform_points(map_T_base_link, point_bl)
01528 close_by_locs = self.locations_man.list_close_by(point_map)
01529
01530
01531
01532
01533
01534 if True:
01535
01536 rospy.loginfo('Select task type:')
01537 for i, ttype in enumerate(self.locations_man.task_types):
01538 print i, ttype
01539 task_type = self.locations_man.task_types[int(raw_input())]
01540 rospy.loginfo('Selected task %s' % task_type)
01541
01542
01543 self.driving_posture(task_type)
01544
01545
01546
01547
01548
01549 self.manipulation_posture(task_type)
01550 point_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), point_map)
01551 self.look_at(point_bl, False)
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575 def has_pos_and_neg(labels):
01576 if np.sum(labels == r3d.POSITIVE) > 0 and np.sum(labels == r3d.NEGATIVE) > 0:
01577 return True
01578 else:
01579 return False
01580
01581 def any_pos_sf(labels_mat):
01582 if np.any(r3d.POSITIVE == labels_mat):
01583 return True
01584 return False
01585
01586
01587 location_name = raw_input('Enter a name for this location:\n')
01588 ctask_type = self.locations_man.get_complementary_task(task_type)
01589 t_current_map, r_current_map = self.robot.base.get_pose()
01590 task_id = self.locations_man.create_new_location(task_type,
01591 np.matrix([0,0,0.]).T, [t_current_map, r_current_map], name=location_name)
01592 ctask_id = self.locations_man.create_new_location(ctask_type,
01593 np.matrix([0,0,0.]).T, [t_current_map, r_current_map], name=location_name)
01594
01595
01596
01597 point_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), point_map)
01598 self.look_at(point_bl, True)
01599 ret_dict_action = self.seed_dataset_explore(task_id, ctask_id, point_bl, stop_fun=has_pos_and_neg, should_reset=True)
01600 dset_action = ret_dict_action['features']
01601 dset_undo = ret_dict_action['features_undo']
01602 undo_point_bl = ret_dict_action['undo_point']
01603
01604
01605
01606
01607 rospy.loginfo('====================================================')
01608 rospy.loginfo('Don\'t have enough data for reverse action')
01609 if (self.locations_man.data[ctask_id]['dataset'] == None) or \
01610 not has_pos_and_neg(self.locations_man.data[ctask_id]['dataset'].outputs.A1):
01611
01612 point_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), point_map)
01613 self.look_at(point_bl, True)
01614 rospy.loginfo('====================================================')
01615 rospy.loginfo('Running seed_dataset_explore on set %s.' % task_id)
01616 ret_dict_action = self.seed_dataset_explore(task_id, None, point_bl, stop_fun=any_pos_sf, should_reset=False)
01617 undo_point_bl = ret_dict_action['undo_point']
01618
01619
01620 self.look_at(point_bl, True)
01621
01622 rospy.loginfo('====================================================')
01623 rospy.loginfo('Running seed_dataset_explore on set %s.' % ctask_id)
01624 ret_dict_undo = self.seed_dataset_explore(ctask_id, task_id, undo_point_bl, stop_fun=has_pos_and_neg)
01625 if dset_undo == None:
01626 dset_undo = ret_dict_undo['features']
01627
01628
01629 tdataset = self.locations_man.data[task_id]['dataset']
01630 tpoint_bl = tdataset.pt3d[:, np.where(tdataset.outputs == r3d.POSITIVE)[1].A1[0]]
01631 self.balance_positives_and_negatives(tdataset)
01632
01633 cdataset = self.locations_man.data[ctask_id]['dataset']
01634 cpoint_bl = cdataset.pt3d[:, np.where(cdataset.outputs == r3d.POSITIVE)[1].A1[0]]
01635 self.balance_positives_and_negatives(cdataset)
01636
01637 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
01638 point_success_map = tfu.transform_points(map_T_base_link, tpoint_bl)
01639 cpoint_success_map = tfu.transform_points(map_T_base_link, cpoint_bl)
01640
01641
01642 self.locations_man.set_center(task_id, point_success_map)
01643 self.locations_man.set_center(ctask_id, cpoint_success_map)
01644 self.locations_man.data[task_id]['complementary_task_id'] = ctask_id
01645 self.locations_man.data[ctask_id]['complementary_task_id'] = task_id
01646
01647 self.locations_man.update_execution_record(task_id, 1.)
01648 self.locations_man.update_execution_record(ctask_id, 1.)
01649
01650
01651
01652
01653
01654
01655 self.locations_man.train(task_id, dset_action, save_pca_images=True)
01656 self.locations_man.train(ctask_id, dset_undo, save_pca_images=True)
01657 self.locations_man.save_database()
01658 rospy.loginfo('Done initializing new location!')
01659 self.driving_posture(task_type)
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689 def move_base_planner(self, trans, rot):
01690
01691 p_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), np.matrix(trans).T)
01692
01693 self.turn_to_point(p_bl)
01694 rvalue = self.robot.base.set_pose(trans, rot, '/map', block=True)
01695 p_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), np.matrix(trans).T)
01696
01697 self.robot.base.move_to(p_bl[0:2,0], True)
01698 t_end, r_end = self.robot.base.get_pose()
01699 return rvalue==3, np.linalg.norm(t_end[0:2] - np.array(trans)[0:2])
01700
01701 def update_base(self):
01702 tasks = self.locations_man.data.keys()
01703 for i, k in enumerate(tasks):
01704 print i, k
01705 selected_idx = int(raw_input("Select an action to update base location\n"))
01706
01707 t_current_map, r_current_map = self.robot.base.get_pose()
01708 task_id = tasks[selected_idx]
01709 self.locations_man.update_base_pose(task_id, [t_current_map, r_current_map])
01710 self.locations_man.save_database()
01711
01712
01713
01714
01715 def scenario_practice_run_mode(self):
01716 rospy.loginfo('===================================================')
01717 rospy.loginfo('= Practice Mode! =')
01718 rospy.loginfo('===================================================')
01719
01720
01721 ulocs = self.unreliable_locs()
01722 rospy.loginfo('%d out of %d locs in database are unreliable' \
01723 % (len(ulocs), len(self.locations_man.data.keys())))
01724
01725
01726 tasks = self.locations_man.data.keys()
01727 for i, k in enumerate(tasks):
01728 print i, k
01729
01730
01731 selected_idx = int(raw_input("Select a location to execute action\n"))
01732
01733
01734 tid = tasks[selected_idx]
01735 rospy.loginfo('selected %s' % tid)
01736
01737
01738
01739 if not self.locations_man.data[tid].has_key('practice_locations'):
01740
01741
01742 map_points = []
01743 for i in range(4):
01744 raw_input('%d move robot to desired position\n' % i)
01745 rospy.sleep(1.)
01746
01747 t_current_map, r_current_map = self.robot.base.get_pose()
01748 map_points.append([t_current_map, r_current_map])
01749 self.locations_man.data[tid]['practice_locations'] = map_points
01750 self.locations_man.data[tid]['practice_locations_history'] = np.zeros((1, len(map_points)))
01751 self.locations_man.data[tid]['practice_locations_convergence'] = np.zeros((1, len(map_points)))
01752
01753 self.locations_man.save_database()
01754
01755
01756 if not self.locations_man.data[tid].has_key('base_pose'):
01757 raw_input('move robot to desired end position\n')
01758 trans, rot_quat = self.robot.base.get_pose()
01759 self.locations_man.data[tid]['base_pose'] = [trans, rot_quat]
01760 self.locations_man.save_database()
01761
01762 point_map = self.locations_man.data[tid]['center']
01763 task_type = self.locations_man.data[tid]['task']
01764 points_added_history = []
01765
01766 unexplored_locs = np.where(self.locations_man.data[tid]['practice_locations_history'] == 0)[1]
01767 unconverged_locs = np.where(self.locations_man.data[tid]['practice_locations_convergence'] == 0)[1]
01768 rospy.loginfo("Location history: %s" % str(self.locations_man.data[tid]['practice_locations_history']))
01769 run_loop = True
01770
01771
01772
01773 if unexplored_locs.shape[0] == len(self.locations_man.data[tid]['practice_locations']):
01774 pidx = 3
01775
01776
01777 elif unexplored_locs.shape[0] > 0:
01778 pidx = unexplored_locs[0]
01779 rospy.loginfo("Resuming training from last unexplored location")
01780
01781
01782 elif unconverged_locs.shape[0] > 0:
01783 pidx = unconverged_locs[0]
01784 rospy.loginfo("Resuming training from unconverged location")
01785
01786
01787 else:
01788 rospy.loginfo("WARNING: no unexplored or unconverged location")
01789 pidx = 3
01790 self.locations_man.data[tid]['practice_locations_convergence'][0, pidx] = 0
01791
01792
01793 while True:
01794
01795 if self.locations_man.data[tid]['practice_locations_convergence'][0, pidx] == 0:
01796
01797 self.driving_posture(task_type)
01798
01799
01800 self.robot.sound.say('Driving to practice location')
01801 rvalue, dist = self.move_base_planner(*self.locations_man.data[tid]['practice_locations'][pidx])
01802 rospy.loginfo('move ret value %s dist %f' % (str(rvalue), dist))
01803
01804
01805 self.robot.sound.say('Driving to mechanism location')
01806 rvalue, dist = self.move_base_planner(*self.locations_man.data[tid]['base_pose'])
01807
01808
01809 bl_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01810 point_bl = tfu.transform_points(bl_T_map, point_map)
01811 ret = self.location_approach_driving(task_type, point_bl)
01812 self.robot.base.set_pose(self.robot.base.get_pose()[0], self.locations_man.data[tid]['base_pose'][1], 'map')
01813 if not ret[0]:
01814 rospy.loginfo('Driving failed!! Resetting.')
01815
01816
01817 continue
01818
01819 self.manipulation_posture(task_type)
01820 bl_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01821 point_bl = tfu.transform_points(bl_T_map, point_map)
01822 self.look_at(point_bl, False)
01823
01824
01825 self.robot.sound.say('practicing')
01826 ctid = self.locations_man.data[tid]['complementary_task_id']
01827
01828 points_before_t = self.locations_man.data[tid]['dataset'].inputs.shape[1]
01829 points_before_ct = self.locations_man.data[ctid]['dataset'].inputs.shape[1]
01830
01831 points_added = self.practice(tid, ctid, point_bl)
01832
01833 points_added_history.append(points_added)
01834 points_after_t = self.locations_man.data[tid]['dataset'].inputs.shape[1]
01835 points_after_ct = self.locations_man.data[ctid]['dataset'].inputs.shape[1]
01836 self.locations_man.record_time(tid, 'num_points_added_' + tid, points_after_t - points_before_t)
01837 self.locations_man.record_time(ctid, 'num_points_added_' + tid, points_after_ct - points_before_ct)
01838
01839 self.locations_man.data[tid]['practice_locations_history'][0, pidx] += 1
01840
01841
01842 if points_added == 0:
01843 self.locations_man.data[tid]['practice_locations_convergence'][0, pidx] = 1
01844 rospy.loginfo('===================================================')
01845 rospy.loginfo('= LOCATION CONVERGED ')
01846 rospy.loginfo('Converged locs: %s' % str(self.locations_man.data[tid]['practice_locations_convergence']))
01847 rospy.loginfo('using instances %d points added' % (points_after_t - points_before_t))
01848 rospy.loginfo('history %s' % str(points_added_history))
01849 rospy.loginfo('number of iterations it took %s' % str(np.sum(points_added_history)))
01850 rospy.loginfo('number of datapoints %s' % str(self.locations_man.data[tid]['dataset'].outputs.shape))
01851 rospy.loginfo('===================================================')
01852 if np.where(self.locations_man.data[tid]['practice_locations_convergence'] == 0)[1].shape[0] <= 0:
01853 break
01854 else:
01855 rospy.loginfo('===================================================')
01856 rospy.loginfo('= Scan converged!')
01857 rospy.loginfo('Converged locs: %s' % str(self.locations_man.data[tid]['practice_locations_convergence']))
01858 rospy.loginfo('using instances %d points added' % (points_after_t - points_before_t))
01859 rospy.loginfo('history %s' % str(points_added_history))
01860 rospy.loginfo('number of iterations so far %s' % str(np.sum(points_added_history)))
01861 rospy.loginfo('number of datapoints %s' % str(self.locations_man.data[tid]['dataset'].outputs.shape))
01862 rospy.loginfo('===================================================')
01863
01864 pidx = (pidx + 1) % len(self.locations_man.data[tid]['practice_locations'])
01865 self.locations_man.save_database()
01866
01867 self.driving_posture(task_type)
01868
01869
01870
01871
01872
01873
01874 def scenario_user_select_location(self, save, user_study):
01875 rospy.loginfo('===================================================')
01876 rospy.loginfo('= User selection mode! =')
01877 rospy.loginfo('===================================================')
01878 tasks = self.locations_man.data.keys()
01879 for i, k in enumerate(tasks):
01880 print i, k
01881
01882 tid = tasks[int(raw_input())]
01883 task_type = self.locations_man.data[tid]['task']
01884 rospy.loginfo('User selected %s' % tid)
01885
01886
01887
01888
01889
01890 if not self.locations_man.data[tid].has_key('execute_locations'):
01891 self.locations_man.data[tid]['execute_locations'] = []
01892 t_current_map, r_current_map = self.robot.base.get_pose()
01893 self.locations_man.data[tid]['execute_locations'].append([t_current_map, r_current_map])
01894 if not user_study:
01895 self.locations_man.save_database()
01896
01897 point_map = self.locations_man.data[tid]['center']
01898
01899 self.robot.sound.say('Driving')
01900 self.driving_posture(task_type)
01901 rvalue, dist = self.move_base_planner(*self.locations_man.data[tid]['base_pose'])
01902 if not rvalue:
01903 self.robot.sound.say('unable to plan to location')
01904 return False
01905
01906 bl_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01907 point_bl = tfu.transform_points(bl_T_map, point_map)
01908 ret = self.location_approach_driving(task_type, point_bl)
01909 self.robot.base.set_pose(self.robot.base.get_pose()[0],
01910 self.locations_man.data[tid]['base_pose'][1], 'map')
01911 if not ret[0]:
01912 self.robot.sound.say('unable to get to location!')
01913 return False
01914
01915 self.manipulation_posture(task_type)
01916 bl_T_map = tfu.transform('base_link', 'map', self.tf_listener)
01917 point_bl = tfu.transform_points(bl_T_map, point_map)
01918 self.look_at(point_bl, False)
01919
01920 self.robot.sound.say('Executing behavior')
01921 self.execute_behavior(tid, point_bl, save, user_study=user_study)
01922 self.driving_posture(task_type)
01923
01924
01925 def unreliable_locs(self):
01926 tasks = self.locations_man.data.keys()
01927 utid = []
01928 for tid in tasks:
01929 if not self.locations_man.is_reliable(tid):
01930 utid.append(tid)
01931 return utid
01932
01933
01934
01935 def click_cb(self, point_bl):
01936
01937
01938
01939
01940
01941
01942 mode = 'location_execute'
01943
01944
01945 if point_bl!= None:
01946 print 'Got point', point_bl.T
01947
01948 if mode == 'location_execute':
01949 self.scenario_user_clicked_at_location(point_bl)
01950
01951
01952
01953
01954
01955
01956
01957
01958
01959
01960
01961
01962
01963
01964
01965
01966
01967
01968
01969
01970
01971
01972
01973
01974
01975
01976
01977
01978
01979
01980
01981
01982
01983
01984
01985
01986
01987
01988
01989
01990
01991
01992
01993 if mode == 'live_label':
01994
01995 light_switch_beh = ft.partial(self.light_switch1,
01996 point_offset=np.matrix([0,0,.03]).T,
01997 press_contact_pressure=300,
01998
01999 press_pressure=3500,
02000 press_distance=np.matrix([0,0,-.15]).T,
02001 visual_change_thres=.03)
02002 point_map = tfu.transform_points(tfu.transform('map', 'base_link', self.tf_listener), point_bl)
02003
02004 while not rospy.is_shutdown():
02005 point_bl_cur = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), point_map)
02006 self.execute_behavior(point_bl_cur, light_switch_beh, 'light_switch')
02007
02008 if mode == 'capture data':
02009 self.robot.head.look_at(point_bl, 'base_link', True)
02010 self.robot.sound.say("taking a scan")
02011
02012
02013
02014 rdict = self.feature_ex.read()['rdict']
02015
02016 self.record_perceptual_data(point_bl, self.OPTICAL_FRAME, rdict=rdict)
02017 self.robot.sound.say("saved scan")
02018
02019 if mode == 'light switch':
02020 point_offset = np.matrix([0, 0, 0.03]).T
02021 success, _ = self.light_switch1(point_bl, point_offset=point_offset, \
02022 press_contact_pressure=300,\
02023
02024 press_pressure=3500, press_distance=np.matrix([0,0,-.15]).T, \
02025 visual_change_thres=.03)
02026
02027 self.behaviors.movement.pressure_listener.rezero()
02028 self.behaviors.movement.set_movement_mode_cart()
02029 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
02030
02031
02032
02033
02034
02035
02036
02037
02038
02039
02040
02041
02042
02043
02044
02045
02046
02047
02048
02049
02050
02051
02052
02053
02054
02055
02056
02057
02058
02059
02060
02061 def run(self, mode, save, user_study):
02062
02063
02064
02065
02066
02067
02068
02069
02070 r = rospy.Rate(10)
02071 rospy.loginfo('Ready.')
02072 while not rospy.is_shutdown():
02073 r.sleep()
02074 if mode == 'practice':
02075 self.scenario_practice_run_mode()
02076
02077 if mode == 'execute':
02078 t = time.time()
02079 self.scenario_user_select_location(save, user_study)
02080 t2 = time.time()
02081 print '>> That took', t2 - t, 'seconds'
02082 exit()
02083
02084 if mode == 'update_base':
02085 self.update_base()
02086
02087
02088
02089
02090
02091
02092
02093
02094
02095
02096
02097
02098
02099
02100
02101
02102
02103
02104
02105
02106
02107
02108
02109
02110
02111
02112
02113
02114
02115
02116
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127
02128
02129
02130
02131
02132
02133
02134
02135
02136
02137
02138
02139
02140
02141
02142
02143
02144 def record_perceptual_data(self, point3d_bl, image_frame, rdict=None, folder_name=None):
02145 rospy.loginfo('saving dataset..')
02146
02147 if rdict == None:
02148 rospy.loginfo('Getting a kinect reading')
02149 rdict = self.feature_ex.read()['rdict']
02150
02151 kimage = rdict['image']
02152 rospy.loginfo('Waiting for calibration.')
02153 while not self.feature_ex.cal.has_msg:
02154 time.sleep(.1)
02155
02156
02157 rospy.loginfo('Getting transforms.')
02158
02159 k_T_bl = tfu.transform(image_frame, '/base_link', self.tf_listener)
02160
02161 tstring = time.strftime('%A_%m_%d_%Y_%I_%M%p')
02162 kimage_name = '%s_highres.png' % tstring
02163 pickle_fname = '%s_interest_point_dataset.pkl' % tstring
02164 if folder_name != None:
02165 try:
02166 os.mkdir(folder_name)
02167 except OSError, e:
02168 print e
02169 kimage_name = pt.join(folder_name, kimage_name)
02170 pickle_fname = pt.join(folder_name, pickle_fname)
02171
02172 rospy.loginfo('Saving images (basename %s)' % tstring)
02173 cv.SaveImage(kimage_name, kimage)
02174 rospy.loginfo('Saving pickles')
02175
02176 data_pkl = {'touch_point': point3d_bl,
02177 'points3d': rdict['points3d'],
02178 'image': kimage_name,
02179 'cal': self.feature_ex.cal,
02180 'k_T_bl': k_T_bl}
02181
02182
02183 ut.save_pickle(data_pkl, pickle_fname)
02184 print 'Recorded to', pickle_fname
02185 return pickle_fname, kimage_name
02186
02187
02188
02189
02190
02191
02192
02193
02194
02195
02196
02197
02198
02199
02200
02201
02202
02203
02204
02205
02206
02207
02208
02209
02210
02211
02212
02213
02214
02215
02216
02217
02218
02219
02220
02221
02222
02223
02224
02225
02226
02227
02228 def practice(self, task_id, ctask_id, point3d_bl, stop_fun=None, params=None,
02229 negative_cut_off=.5, resolution=.01, max_samples=5):
02230 if params == None:
02231 params = r3d.Recognize3DParam()
02232 params.uncertainty_x = 1.
02233
02234 params.uncertainty_z = .04
02235 params.uni_mix = .1
02236 pstart = time.time()
02237
02238 kdict, image_name = self.read_features_save(task_id, point3d_bl, params)
02239
02240
02241 behavior = self.get_behavior_by_task(self.locations_man.data[task_id]['task'])
02242 head_pose = self.robot.head.pose()
02243
02244 kdict['image_T_bl'] = tfu.transform(self.OPTICAL_FRAME, 'base_link', self.tf_listener)
02245 point3d_img = tfu.transform_points(kdict['image_T_bl'], point3d_bl)
02246 point2d_img = self.feature_ex.cal.project(point3d_img)
02247
02248 labels = []
02249 points3d_tried = []
02250 points2d_tried = []
02251 converged = False
02252 indices_added = []
02253 reset_times = []
02254
02255
02256 while True:
02257 if stop_fun != None and stop_fun(np.matrix(labels)):
02258 rospy.loginfo('Stop satisfied told us to stop loop!')
02259 break
02260
02261 if stop_fun == None and len(indices_added) > params.max_points_per_site:
02262 rospy.loginfo('practice: added enough points from this scan. Limit is %d points.' % params.max_points_per_site)
02263 break
02264
02265
02266
02267
02268
02269 iter_start = time.time()
02270 print 'input to inverse_indices'
02271 print '>>', indices_added, kdict['instances'].shape[1]
02272 remaining_pt_indices = r3d.inverse_indices(indices_added, kdict['instances'].shape[1])
02273 remaining_instances = kdict['instances'][:, remaining_pt_indices]
02274
02275
02276 ridx, selected_dist, converged = self.locations_man.learners[task_id].select_next_instances_no_terminate(remaining_instances)
02277
02278
02279
02280
02281
02282 selected_idx = remaining_pt_indices[ridx[0]]
02283
02284 indices_added.append(selected_idx)
02285
02286
02287
02288
02289 img = cv.CloneMat(kdict['image'])
02290
02291 r3d.draw_points(img, point2d_img, [255, 0, 0], 6, 2)
02292
02293
02294 r3d.draw_points(img, kdict['points2d']+np.matrix([1,1.]).T, [255, 255, 255], 4, -1)
02295
02296 if len(points2d_tried) > 0:
02297 _, pos_exp, neg_exp = separate_by_labels(np.column_stack(points2d_tried), np.matrix(labels))
02298 r3d.draw_points(img, pos_exp, [50, 255, 0], 8, 1)
02299 r3d.draw_points(img, neg_exp, [50, 0, 255], 8, 1)
02300
02301 predictions = np.matrix(self.locations_man.learners[task_id].classify(kdict['instances']))
02302 _, pos_pred, neg_pred = separate_by_labels(kdict['points2d'], predictions)
02303 r3d.draw_points(img, pos_pred, [255, 204, 51], 3, -1)
02304 r3d.draw_points(img, neg_pred, [51, 204, 255], 3, -1)
02305
02306
02307 r3d.draw_points(img, kdict['points2d'][:, selected_idx], [255, 51, 204], 8, -1)
02308 self.locations_man.publish_image(task_id, img, postfix='_practice_pick')
02309
02310
02311
02312
02313 self.robot.head.set_pose(head_pose, 1)
02314
02315 if np.linalg.norm(kdict['points3d'][:, selected_idx] - point3d_bl) > negative_cut_off:
02316 rospy.loginfo('#########################################')
02317 rospy.loginfo('Point outside of negative cut off!! Eliminating %s' % (str(kdict['points3d'][:, selected_idx].T)))
02318 rospy.loginfo('#########################################')
02319 success = False
02320 else:
02321 if len(points3d_tried) > 0:
02322 existing_pts_tree = sp.KDTree(np.array(np.column_stack(points3d_tried).T))
02323 close_by_indices = existing_pts_tree.query_ball_point(np.array(kdict['points3d'][:, selected_idx]).T, resolution)[0]
02324 if len(close_by_indices) > 0:
02325
02326
02327 rospy.loginfo('#########################################')
02328 rospy.loginfo('Point within resolutio of existing point.')
02329 rospy.loginfo('#########################################')
02330 continue
02331
02332
02333
02334
02335
02336
02337 else:
02338 success, _, undo_point_bl = behavior(kdict['points3d'][:, selected_idx])
02339 else:
02340 success, _ , undo_point_bl = behavior(kdict['points3d'][:, selected_idx])
02341
02342
02343 if success:
02344 color = [0,255,0]
02345 label = r3d.POSITIVE
02346 rospy.loginfo('=============================================')
02347 rospy.loginfo('>> behavior successful')
02348 rospy.loginfo('=============================================')
02349 self.robot.sound.say('action succeeded')
02350 else:
02351 label = r3d.NEGATIVE
02352 color = [0,0,255]
02353 rospy.loginfo('=============================================')
02354 rospy.loginfo('>> behavior NOT successful')
02355 rospy.loginfo('=============================================')
02356 self.robot.sound.say('action failed')
02357
02358
02359
02360
02361 labels.append(label)
02362 points3d_tried.append(kdict['points3d'][:, selected_idx])
02363 points2d_tried.append(kdict['points2d'][:, selected_idx])
02364
02365 datapoint = {'instances': kdict['instances'][:, selected_idx],
02366 'points2d': kdict['points2d'][:, selected_idx],
02367 'points3d': kdict['points3d'][:, selected_idx],
02368 'sizes': kdict['sizes'],
02369 'labels': np.matrix([label])
02370 }
02371 self.locations_man.add_perceptual_data(task_id, datapoint)
02372 self.locations_man.save_database()
02373 self.locations_man.train(task_id, kdict)
02374
02375
02376
02377
02378
02379 reset_start = time.time()
02380 if success and ctask_id != None:
02381 def any_pos_sf(labels_mat):
02382 if np.any(r3d.POSITIVE == labels_mat):
02383 return True
02384 return False
02385
02386 undo_point_map = self.locations_man.data[ctask_id]['center']
02387 undo_point_bl0 = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), undo_point_map)
02388
02389
02390 num_points_added = self.practice(ctask_id, None, undo_point_bl0, stop_fun=any_pos_sf)
02391
02392
02393
02394
02395
02396
02397
02398
02399
02400
02401
02402
02403
02404 reset_end = time.time()
02405
02406
02407 predictions = np.matrix(self.locations_man.learners[task_id].classify(kdict['instances']))
02408
02409
02410
02411
02412 img = cv.CloneMat(kdict['image'])
02413
02414
02415 r3d.draw_points(img, point2d_img, [255, 0, 0], 6, 2)
02416
02417
02418 r3d.draw_points(img, kdict['points2d']+np.matrix([1,1.]).T, [255, 255, 255], 4, -1)
02419
02420
02421 _, pos_exp, neg_exp = separate_by_labels(np.column_stack(points2d_tried), np.matrix(labels))
02422 r3d.draw_points(img, pos_exp, [50, 255, 0], 9, 1)
02423 r3d.draw_points(img, neg_exp, [50, 0, 255], 9, 1)
02424
02425 _, pos_pred, neg_pred = separate_by_labels(kdict['points2d'], predictions)
02426 r3d.draw_points(img, pos_pred, [255, 204, 51], 3, -1)
02427 r3d.draw_points(img, neg_pred, [51, 204, 255], 3, -1)
02428
02429
02430 r3d.draw_points(img, points2d_tried[-1], color, 8, -1)
02431 self.locations_man.publish_image(task_id, img, postfix='_practice_result')
02432
02433 pkname = pt.join(task_id, time.strftime('%A_%m_%d_%Y_%I_%M_%S%p') + '.pkl')
02434
02435 ut.save_pickle({'image': image_name,
02436 'pos': pos_exp,
02437 'neg': neg_exp,
02438 'pos_pred': pos_pred,
02439 'neg_pred': neg_pred,
02440 'tried': (points2d_tried[-1], label),
02441 'center': point2d_img}, pkname)
02442
02443
02444
02445
02446 reset_time = reset_end - reset_start
02447 loop_time = (time.time() - iter_start) - (reset_end - reset_start)
02448 reset_times.append(reset_time)
02449 print '**********************************************************'
02450 print 'Loop took %.3f seconds' % loop_time
02451 print '**********************************************************'
02452 self.locations_man.record_time(task_id, 'practice_loop_time', loop_time)
02453
02454 if np.any(r3d.POSITIVE == np.matrix(labels)):
02455 self.locations_man.update_execution_record(task_id, 1)
02456
02457 print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
02458 print 'returning from', task_id
02459 print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
02460 pend = time.time()
02461 practice_time = pend - pstart - np.sum(reset_times)
02462 self.locations_man.record_time(task_id, 'practice_func_time', practice_time)
02463 return len(indices_added)
02464
02465
02466 def balance_positives_and_negatives(self, dataset):
02467
02468 poslist = np.where(dataset.outputs == r3d.POSITIVE)[1].A1.tolist()
02469 neglist = np.where(dataset.outputs == r3d.NEGATIVE)[1].A1.tolist()
02470 npoint = min(len(poslist), len(neglist))
02471 assert(npoint > 0)
02472 sindices = poslist[:npoint]+neglist[:npoint]
02473 dataset.pt3d = dataset.pt3d[:, sindices]
02474 dataset.pt2d = dataset.pt2d[:, sindices]
02475 dataset.outputs = dataset.outputs[:, sindices]
02476 dataset.inputs = dataset.inputs[:, sindices]
02477
02478 def profile_me(self, task_id, point_bl):
02479 for i in range(2):
02480 params = r3d.Recognize3DParam()
02481 params.uncertainty_x = 1.
02482 params.uncertainty_y = .04
02483 params.uncertainty_z = .04
02484 params.n_samples = 5000
02485 params.uni_mix = 0.1
02486 print 'PROFILE_ME ITERATION', i
02487 fea_dict, image_name = self.read_features_save(task_id, point_bl, params)
02488
02489
02490
02491
02492
02493 def seed_dataset_explore(self, task_id, ctask_id, point_bl, stop_fun,
02494 max_retries=30, closeness_tolerance=.01, positive_escape=.08, should_reset=False):
02495
02496
02497
02498
02499
02500
02501
02502
02503
02504
02505 self.look_at(point_bl, True)
02506 rospy.sleep(2.)
02507 params = r3d.Recognize3DParam()
02508 params.uncertainty_x = 1.
02509 params.uncertainty_y = .04
02510 params.uncertainty_z = .04
02511 params.n_samples = 800
02512 params.uni_mix = 0.1
02513
02514
02515
02516
02517
02518
02519
02520 fea_dict, image_name = self.read_features_save(task_id, point_bl, params)
02521
02522 params2 = r3d.Recognize3DParam()
02523 params2.n_samples = 5000
02524 fea_dict2, image_name2 = self.read_features_save(task_id, point_bl, params2)
02525
02526 behavior = self.get_behavior_by_task(self.locations_man.data[task_id]['task'])
02527 image_T_bl = tfu.transform(self.OPTICAL_FRAME, 'base_link', self.tf_listener)
02528 fea_dict['image_T_bl'] = image_T_bl
02529
02530
02531 dists = ut.norm(fea_dict['points3d'] - point_bl)
02532 ordering = np.argsort(dists).A1
02533 points3d_sampled = fea_dict['points3d'][:, ordering]
02534 points2d_sampled = fea_dict['points2d'][:, ordering]
02535 instances_sampled = fea_dict['instances'][:, ordering]
02536 start_pose = self.robot.head.pose()
02537
02538
02539 point3d_img = tfu.transform_points(fea_dict['image_T_bl'], point_bl)
02540 point2d_img = self.feature_ex.cal.project(point3d_img)
02541
02542
02543 undo_ret = None
02544 points3d_tried = []
02545 points2d_tried = []
02546 labels = []
02547 sampled_idx = 0
02548 iter_count = 0
02549 need_reset = False
02550 behavior_end_state = False
02551 undo_point_bl = point_bl
02552
02553
02554 while iter_count < max_retries and not stop_fun(np.matrix(labels)) and sampled_idx < points3d_sampled.shape[1]:
02555
02556
02557
02558 if len(points3d_tried) > 0 and \
02559 np.any(ut.norm(np.column_stack(points3d_tried) - points3d_sampled[:, sampled_idx]) < closeness_tolerance):
02560 sampled_idx = sampled_idx + 1
02561 continue
02562
02563 if len(labels) > 0 and np.sum(labels) == len(labels) and\
02564 np.any(ut.norm(np.column_stack(points3d_tried) - points3d_sampled[:, sampled_idx]) < positive_escape):
02565
02566 sampled_idx = sampled_idx + 1
02567 continue
02568
02569
02570
02571
02572 self.robot.head.set_pose(start_pose, 1)
02573
02574 success, reason, undo_point_bl = behavior(points3d_sampled[:, sampled_idx])
02575 rospy.loginfo('======================================')
02576 rospy.loginfo('%s was %s' % (task_id, str(success)))
02577 rospy.loginfo('======================================')
02578 behavior_end_state = success
02579
02580
02581
02582
02583
02584 if success:
02585 label = r3d.POSITIVE
02586 need_reset = True
02587 else:
02588 label = r3d.NEGATIVE
02589 need_reset = False
02590
02591 points3d_tried.append(points3d_sampled[:, sampled_idx])
02592 points2d_tried.append(points2d_sampled[:, sampled_idx])
02593 labels.append(label)
02594 datapoint = {'instances': instances_sampled[:, sampled_idx],
02595 'points3d': points3d_sampled[:, sampled_idx],
02596 'points2d': points2d_sampled[:, sampled_idx],
02597 'sizes': fea_dict['sizes'],
02598 'labels': np.matrix([label])}
02599 self.locations_man.add_perceptual_data(task_id, datapoint)
02600 self.locations_man.save_database()
02601
02602 iter_count = iter_count + 1
02603 sampled_idx = sampled_idx + 1
02604
02605
02606
02607
02608 img = cv.CloneMat(fea_dict['image'])
02609 r3d.draw_points(img, points2d_sampled+np.matrix([1,1.]).T, [0, 0, 0], 3, -1)
02610 r3d.draw_points(img, points2d_sampled, [255, 255, 255], 3, -1)
02611 _, pos_points, neg_points = separate_by_labels(np.column_stack(points2d_tried), np.matrix(labels))
02612 r3d.draw_points(img, point2d_img, [255, 0, 0], 6, 2)
02613 r3d.draw_points(img, pos_points, [0, 255, 0], 4, -1)
02614 r3d.draw_points(img, neg_points, [0, 0, 255], 4, -1)
02615 r3d.draw_points(img, points2d_tried[-1], [0, 184, 245], 6, -1)
02616 self.locations_man.publish_image(task_id, img)
02617
02618 pkname = pt.join(task_id, time.strftime('%A_%m_%d_%Y_%I_%M_%S%p') + '_seed.pkl')
02619 ut.save_pickle({'image': image_name,
02620 'pos': pos_points,
02621 'neg': neg_points,
02622
02623
02624 'tried': [points2d_tried[-1], label],
02625 'center': point2d_img}, pkname)
02626
02627
02628
02629
02630
02631
02632
02633
02634 if need_reset and should_reset:
02635 self.robot.head.set_pose(start_pose, 1)
02636 if ctask_id != None:
02637
02638 def any_pos_sf(labels_mat):
02639 if np.any(r3d.POSITIVE == labels_mat):
02640 return True
02641 return False
02642
02643
02644 ctask_point = undo_point_bl
02645 undo_ret = self.seed_dataset_explore(ctask_id, None, ctask_point, stop_fun=any_pos_sf,
02646 max_retries=max_retries, closeness_tolerance=closeness_tolerance,
02647 should_reset=False)
02648 need_reset = False
02649 if undo_ret['end_state']:
02650 behavior_end_state = False
02651 else:
02652 behavior_end_state = True
02653
02654 rospy.loginfo('Tried %d times' % iter_count)
02655 fea_dict_undo = None
02656 if undo_ret != None:
02657 fea_dict_undo = undo_ret['features']
02658
02659 return {'end_state': behavior_end_state, 'features': fea_dict2,
02660 'features_undo': fea_dict_undo, 'undo_point': undo_point_bl}
02661
02662 def read_features_save(self, task_id, point3d_bl, params=None):
02663 self.robot.projector.set(True)
02664 rospy.sleep(2.)
02665 f = self.feature_ex.read(point3d_bl, params=params)
02666 file_name, kimage_name = self.record_perceptual_data(point3d_bl, self.OPTICAL_FRAME, rdict=f['rdict'], folder_name=task_id)
02667 self.robot.projector.set(False)
02668
02669
02670
02671
02672
02673
02674
02675
02676
02677
02678
02679 return f, kimage_name
02680
02681
02682
02683
02684
02685
02686 def execute_behavior(self, task_id, point3d_bl, save, max_retries=15, closeness_tolerance=.01, user_study=False):
02687 if user_study:
02688 rospy.loginfo('=====================================================')
02689 rospy.loginfo('user_study is True. Trying to fail on first attempt!')
02690 rospy.loginfo('=====================================================')
02691
02692 params = r3d.Recognize3DParam()
02693 params.uncertainty_x = 1.
02694 params.uncertainty_z = .03
02695 params.uni_mix = .1
02696 params.n_samples = 1500
02697
02698 kdict, image_name = self.read_features_save(task_id, point3d_bl, params)
02699 kdict['image_T_bl'] = tfu.transform(self.OPTICAL_FRAME, 'base_link', self.tf_listener)
02700 point3d_img = tfu.transform_points(kdict['image_T_bl'], point3d_bl)
02701 point2d_img = self.feature_ex.cal.project(point3d_img)
02702 head_pose = self.robot.head.pose()
02703
02704
02705
02706 predictions = np.matrix(self.locations_man.learners[task_id].classify(kdict['instances']))
02707 pos_indices = np.where(r3d.POSITIVE == predictions)[1].A1
02708 loc2d_max = None
02709 try_num = 0
02710 updated_execution_record = False
02711 if user_study:
02712 first_time = True
02713 else:
02714 first_time = False
02715
02716
02717 if len(pos_indices) > 0:
02718 indices_added = []
02719 remaining_pt_indices = r3d.inverse_indices(indices_added, kdict['instances'].shape[1])
02720 remaining_instances = kdict['instances'][:, remaining_pt_indices]
02721 behavior = self.get_behavior_by_task(self.locations_man.data[task_id]['task'])
02722
02723 while len(pos_indices) > 0:
02724
02725 print 'Num positive points found:', len(pos_indices)
02726
02727
02728 locs2d = None
02729 if len(pos_indices) > 1:
02730 locs2d = kdict['points2d'][:, pos_indices]
02731 if np.any(np.isnan(locs2d)) or np.any(np.isinf(locs2d)):
02732 pdb.set_trace()
02733 locs2d_indices = np.where(False == np.sum(np.isnan(locs2d), 0))[1].A1
02734 print locs2d[:, locs2d_indices]
02735 loc2d_max, density_image = r3d.find_max_in_density(locs2d[:, locs2d_indices])
02736 cv.SaveImage("execute_behavior.png", 255 * (np.rot90(density_image)/np.max(density_image)))
02737 dists = ut.norm(kdict['points2d'] - loc2d_max)
02738 selected_idx = np.argmin(dists)
02739 if not first_time:
02740 indices_added.append(selected_idx)
02741 else:
02742 selected_idx = pos_indices[0]
02743 loc2d_max = kdict['points2d'][: selected_idx]
02744
02745 selected_3d = kdict['points3d'][:, selected_idx]
02746
02747
02748 img = cv.CloneMat(kdict['image'])
02749 r3d.draw_points(img, point2d_img, [255, 0, 0], 10, -1)
02750 r3d.draw_points(img, kdict['points2d'], [51, 204, 255], 3, -1)
02751 if locs2d != None:
02752 r3d.draw_points(img, locs2d, [255, 204, 51], 3, -1)
02753 r3d.draw_points(img, loc2d_max, [255, 204, 51], 10, -1)
02754 self.locations_man.publish_image(task_id, img, postfix='_execute_pick')
02755
02756
02757 self.robot.head.set_pose(head_pose, 1)
02758 print '============================================================'
02759 print '============================================================'
02760 print 'Try number', try_num
02761 print '============================================================'
02762 print '============================================================'
02763 if first_time:
02764 print 'original point selected is', selected_3d.T
02765 selected_3d = selected_3d + np.matrix([0, 0, 0.2]).T
02766 print 'point selected is', selected_3d.T
02767 success, _, point_undo_bl = behavior(selected_3d)
02768
02769
02770 pkname = pt.join(task_id, time.strftime('%A_%m_%d_%Y_%I_%M_%S%p') + '_execute.pkl')
02771 _, pos_pred, neg_pred = separate_by_labels(kdict['points2d'], predictions)
02772 if success:
02773 label = r3d.POSITIVE
02774 else:
02775 label = r3d.NEGATIVE
02776 ut.save_pickle({'image': image_name,
02777 'pos_pred': pos_pred,
02778 'neg_pred': neg_pred,
02779 'tried': [kdict['points2d'][:, selected_idx], label],
02780 'center': point2d_img}, pkname)
02781
02782 try_num = try_num + 1
02783 if not first_time:
02784 if success:
02785
02786 if not updated_execution_record:
02787 self.locations_man.update_execution_record(task_id, 1.)
02788 update_execution_record = True
02789 self.robot.sound.say('action succeeded')
02790 label = r3d.POSITIVE
02791
02792 dataset = self.locations_man.data[task_id]['dataset']
02793 nneg = np.sum(dataset.outputs == r3d.NEGATIVE)
02794 npos = np.sum(dataset.outputs == r3d.POSITIVE)
02795 if nneg > npos and save:
02796 datapoint = {'instances': kdict['instances'][:, selected_idx],
02797 'points2d': kdict['points2d'][:, selected_idx],
02798 'points3d': kdict['points3d'][:, selected_idx],
02799 'sizes': kdict['sizes'],
02800 'labels': np.matrix([label])}
02801 self.locations_man.add_perceptual_data(task_id, datapoint)
02802 if save:
02803 self.locations_man.save_database()
02804 self.locations_man.train(task_id, kdict)
02805 break
02806 else:
02807 self.robot.sound.say('action failed')
02808 label = r3d.NEGATIVE
02809 if not updated_execution_record:
02810 self.locations_man.update_execution_record(task_id, 0.)
02811 update_execution_record = True
02812
02813
02814 datapoint = {'instances': kdict['instances'][:, selected_idx],
02815 'points2d': kdict['points2d'][:, selected_idx],
02816 'points3d': kdict['points3d'][:, selected_idx],
02817 'sizes': kdict['sizes'],
02818 'labels': np.matrix([label])}
02819 self.locations_man.add_perceptual_data(task_id, datapoint)
02820 if save:
02821 self.locations_man.save_database()
02822 self.locations_man.train(task_id, kdict)
02823
02824 if try_num > max_retries:
02825 break
02826
02827
02828 remaining_pt_indices = r3d.inverse_indices(indices_added, kdict['instances'].shape[1])
02829 remaining_instances = kdict['instances'][:, remaining_pt_indices]
02830 predictions = np.matrix(self.locations_man.learners[task_id].classify(remaining_instances))
02831 remaining_pos_indices = np.where(r3d.POSITIVE == predictions)[1].A1
02832 pos_indices = remaining_pt_indices[remaining_pos_indices]
02833 first_time = False
02834
02835
02836 else:
02837 img = cv.CloneMat(kdict['image'])
02838 r3d.draw_points(img, point2d_img, [255, 0, 0], 10, -1)
02839 r3d.draw_points(img, kdict['points2d'], [51, 204, 255], 3, -1)
02840 self.locations_man.publish_image(task_id, img, postfix='_execute_fail')
02841
02842 if not updated_execution_record:
02843 self.locations_man.update_execution_record(task_id, 0.)
02844 update_execution_record = True
02845
02846 self.robot.sound.say("Perception failure. Exploring around expected region.")
02847
02848 if save:
02849 self.locations_man.update_execution_record(task_id, 0.)
02850 def any_pos_sf(labels_mat):
02851 if np.any(r3d.POSITIVE == labels_mat):
02852 return True
02853 return False
02854 ctask_id = self.locations_man.data[task_id]['complementary_task_id']
02855 self.seed_dataset_explore(task_id, ctask_id,
02856 point3d_bl, max_retries=max_retries, stop_fun=any_pos_sf,
02857 closeness_tolerance=closeness_tolerance, should_reset=False)
02858
02859
02860
02861
02862
02863
02864
02865
02866
02867
02868
02869
02870
02871
02872
02873
02874
02875
02876
02877
02878
02879
02880
02881
02882
02883
02884
02885
02886
02887
02888
02889
02890
02891
02892
02893
02894
02895
02896
02897
02898
02899
02900
02901
02902
02903
02904
02905
02906
02907
02908
02909
02910
02911
02912
02913
02914
02915 class TestLearner:
02916
02917 def __init__(self):
02918 self.rec_params = r3d.Recognize3DParam()
02919 self.locations_man = LocationManager('locations_narrow_v11.pkl', rec_params=self.rec_params)
02920
02921 def test_training_set(self):
02922
02923 tasks = self.locations_man.data.keys()
02924 for i, k in enumerate(tasks):
02925 print i, k
02926 task_id = tasks[int(raw_input())]
02927
02928
02929 dataset = self.locations_man.data[task_id]['dataset']
02930 pca = self.locations_man.data[task_id]['pca']
02931
02932 nneg = np.sum(dataset.outputs == r3d.NEGATIVE)
02933 npos = np.sum(dataset.outputs == r3d.POSITIVE)
02934 print '================= Training ================='
02935 print 'NEG examples', nneg
02936 print 'POS examples', npos
02937 print 'TOTAL', dataset.outputs.shape[1]
02938 neg_to_pos_ratio = float(nneg)/float(npos)
02939
02940
02941
02942
02943
02944
02945 all_costs_scores = []
02946 all_ratio_scores = []
02947 all_bandwidth_scores = []
02948 for i in range(40):
02949 percent = .3
02950 negidx = np.where(dataset.outputs==r3d.NEGATIVE)[1].A1
02951 posidx = np.where(dataset.outputs==r3d.POSITIVE)[1].A1
02952 nneg = np.ceil(len(negidx) * percent)
02953 npos = np.ceil(len(posidx) * percent)
02954 negperm = np.random.permutation(range(len(negidx)))
02955 posperm = np.random.permutation(range(len(posidx)))
02956
02957 test_pos_idx = negperm[0:nneg]
02958 test_neg_idx = posperm[0:npos]
02959 test_idx = np.concatenate((test_pos_idx, test_neg_idx))
02960
02961 train_pos_idx = negperm[nneg:]
02962 train_neg_idx = posperm[npos:]
02963 train_idx = np.concatenate((train_pos_idx, train_neg_idx))
02964
02965 test_set = dataset.subset(test_idx)
02966 train_set = dataset.subset(train_idx)
02967
02968
02969 ratio_score = []
02970 ratios = [neg_to_pos_ratio * (float(i+1)/10.) for i in range(10)]
02971 for r in ratios:
02972 print '######################################################'
02973 print 'ratio is ', r
02974 svm_params = '-s 0 -t 2 -g .4 -c 1 '
02975 learner = self.train(train_set, pca, r, svm_params)
02976 predictions = np.matrix(learner.classify(test_set.inputs))
02977 conf_mat = r3d.confusion_matrix(test_set.outputs, predictions)
02978 combined = conf_mat[0,0] + conf_mat[1,1]
02979 print '!!!!!!!!!!!'
02980 print conf_mat
02981 print combined
02982 print '!!!!!!!!!!!'
02983 ratio_score.append(combined)
02984
02985 bandwidth_score = []
02986 bandwidths = []
02987 for i in range(40):
02988 print '######################################################'
02989 g = i * .1
02990 bandwidths.append(g)
02991 print 'g is ', g
02992 svm_params = '-s 0 -t 2 -g %.2f -c 1 ' % g
02993 learner = self.train(train_set, pca, neg_to_pos_ratio, svm_params)
02994 predictions = np.matrix(learner.classify(test_set.inputs))
02995 conf_mat = r3d.confusion_matrix(test_set.outputs, predictions)
02996 combined = conf_mat[0,0] + conf_mat[1,1]
02997 print '!!!!!!!!!!!'
02998 print conf_mat
02999 print combined
03000 print '!!!!!!!!!!!'
03001 bandwidth_score.append(combined)
03002
03003 cost_score = []
03004 costs = []
03005 for i in range(40):
03006 print '######################################################'
03007 cost = i + 1
03008 costs.append(cost)
03009 print 'cost is ', cost
03010 svm_params = '-s 0 -t 2 -g .4 -c %.2f ' % cost
03011 learner = self.train(train_set, pca, neg_to_pos_ratio, svm_params)
03012 predictions = np.matrix(learner.classify(test_set.inputs))
03013 conf_mat = r3d.confusion_matrix(test_set.outputs, predictions)
03014 combined = conf_mat[0,0] + conf_mat[1,1]
03015 print '!!!!!!!!!!!'
03016 print conf_mat
03017 print combined
03018 print '!!!!!!!!!!!'
03019 cost_score.append(combined)
03020
03021
03022
03023
03024
03025
03026
03027
03028
03029
03030
03031
03032 all_costs_scores.append(cost_score)
03033 all_ratio_scores.append(ratio_score)
03034 all_bandwidth_scores.append(bandwidth_score)
03035
03036
03037 print 'Cost score'
03038 print costs
03039 costs_mean = np.sum(np.matrix(all_costs_scores), 0) / float(len(all_costs_scores))
03040 print costs_mean
03041 print np.max(costs_mean)
03042 print costs[np.argmax(costs_mean)]
03043
03044 print 'Ratio score'
03045 print ratios
03046 ratios_mean = np.sum(np.matrix(all_ratio_scores), 0) / float(len(all_ratio_scores))
03047 print ratios_mean
03048 print np.max(ratios_mean)
03049 print ratios[np.argmax(ratios_mean)]
03050
03051 print 'Bandwidth score'
03052 print bandwidths
03053 bandwidths_mean = np.sum(np.matrix(all_bandwidth_scores), 0) / float(len(all_bandwidth_scores))
03054 print bandwidths_mean
03055 print np.max(bandwidths_mean)
03056 print bandwidths[np.argmax(bandwidths_mean)]
03057
03058
03059
03060
03061 def train(self, dataset, pca, neg_to_pos_ratio, svm_params):
03062 weight_balance = ' -w0 1 -w1 %.2f' % neg_to_pos_ratio
03063 previous_learner = None
03064 learner = r3d.SVMPCA_ActiveLearner(use_pca=True,
03065 reconstruction_std_lim=self.rec_params.reconstruction_std_lim,
03066 reconstruction_err_toler=self.rec_params.reconstruction_err_toler,
03067 old_learner=None, pca=pca)
03068
03069 rec_params = self.rec_params
03070 inputs_for_pca = dataset.inputs
03071 learner.train(dataset,
03072 inputs_for_pca,
03073 svm_params + weight_balance,
03074 rec_params.variance_keep)
03075 return learner
03076
03077
03078 def test_display():
03079 rec_params = r3d.Recognize3DParam()
03080 locations_man = LocationManager('locations_narrow_v11.pkl',
03081 rec_params=rec_params)
03082 location_display = LocationDisplay(locations_man)
03083 location_display.run()
03084
03085
03086
03087
03088 def launch():
03089 import optparse
03090 p = optparse.OptionParser()
03091 p.add_option("-d", "--display", action="store_true", default=False)
03092 p.add_option("-p", "--practice", action="store_true", default=False)
03093 p.add_option("-e", "--execute", action="store_true", default=False)
03094 p.add_option("-b", "--base", action="store_true", default=False)
03095 p.add_option("-s", "--static", action="store_true", default=False)
03096 p.add_option("-i", "--init", action="store_true", default=False)
03097 p.add_option("-t", "--test", action="store_true", default=False)
03098 p.add_option("-u", "--user_study", action="store_true", default=False)
03099
03100 opt, args = p.parse_args()
03101 if opt.display:
03102 test_display()
03103 return
03104
03105 if opt.practice or opt.execute or opt.init or opt.base:
03106 l = ApplicationBehaviors()
03107 mode = None
03108 if opt.practice:
03109 mode = 'practice'
03110 if opt.execute:
03111 mode = 'execute'
03112 if opt.init:
03113 mode = 'init'
03114 if opt.base:
03115 mode = 'update_base'
03116 rospy.loginfo('Using mode %s' % mode)
03117 l.run(mode, not opt.static, opt.user_study)
03118
03119 if opt.test:
03120 tl = TestLearner()
03121 tl.test_training_set()
03122
03123
03124 if __name__ == '__main__':
03125 launch()
03126
03127
03128
03129
03130
03131
03132
03133
03134
03135
03136
03137
03138
03139
03140
03141
03142
03143
03144
03145
03146
03147
03148
03149
03150
03151
03152
03153
03154
03155
03156
03157
03158
03159
03160
03161
03162
03163
03164
03165
03166
03167
03168
03169
03170
03171
03172
03173
03174
03175
03176
03177
03178
03179
03180
03181
03182
03183
03184
03185
03186
03187
03188
03189
03190
03191
03192
03193
03194
03195
03196
03197
03198
03199
03200
03201
03202
03203
03204
03205
03206
03207
03208
03209
03210
03211
03212
03213
03214
03215
03216
03217
03218
03219
03220
03221
03222
03223
03224
03225
03226
03227
03228
03229
03230
03231
03232
03233
03234
03235
03236
03237
03238
03239
03240
03241
03242
03243
03244
03245
03246
03247
03248
03249
03250
03251
03252
03253
03254
03255
03256
03257
03258
03259
03260
03261
03262
03263
03264
03265
03266
03267
03268
03269
03270
03271
03272
03273
03274
03275
03276
03277
03278
03279
03280
03281
03282
03283
03284
03285
03286
03287
03288
03289
03290
03291
03292
03293
03294
03295
03296
03297
03298
03299
03300
03301
03302
03303
03304
03305
03306
03307
03308
03309
03310
03311
03312
03313
03314
03315
03316
03317
03318
03319
03320
03321
03322
03323
03324
03325
03326
03327
03328
03329
03330
03331
03332
03333
03334
03335
03336
03337
03338
03339
03340
03341
03342
03343
03344
03345
03346
03347
03348
03349
03350
03351
03352
03353
03354
03355
03356
03357
03358
03359
03360
03361
03362
03363
03364
03365
03366
03367
03368
03369
03370
03371
03372
03373
03374
03375
03376
03377
03378
03379
03380
03381
03382
03383
03384
03385
03386
03387
03388
03389
03390
03391
03392
03393
03394
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404
03405
03406
03407
03408
03409
03410
03411
03412
03413
03414
03415
03416
03417
03418
03419
03420
03421
03422
03423
03424
03425
03426
03427
03428
03429
03430
03431
03432
03433
03434
03435
03436
03437
03438
03439
03440
03441
03442
03443
03444
03445
03446
03447
03448
03449
03450
03451
03452
03453
03454
03455
03456
03457
03458
03459
03460
03461
03462
03463
03464
03465
03466
03467
03468
03469
03470
03471
03472
03473
03474
03475
03476
03477
03478
03479
03480
03481
03482
03483
03484
03485
03486
03487
03488
03489
03490
03491
03492
03493
03494
03495
03496
03497
03498
03499
03500
03501
03502
03503
03504
03505
03506
03507
03508
03509
03510
03511
03512
03513
03514
03515
03516
03517
03518
03519
03520
03521
03522
03523
03524
03525
03526
03527
03528
03529
03530
03531
03532
03533
03534
03535
03536
03537
03538
03539
03540
03541
03542
03543
03544
03545
03546
03547
03548
03549
03550
03551
03552
03553
03554
03555
03556
03557
03558
03559
03560
03561
03562
03563
03564
03565
03566
03567
03568
03569
03570
03571
03572
03573
03574
03575
03576
03577
03578
03579
03580
03581
03582
03583
03584
03585
03586
03587
03588
03589
03590
03591
03592
03593
03594
03595
03596
03597
03598
03599
03600
03601
03602
03603
03604
03605
03606
03607
03608
03609
03610
03611
03612
03613
03614
03615
03616
03617
03618
03619
03620
03621
03622
03623
03624
03625
03626
03627
03628
03629
03630
03631
03632
03633
03634
03635
03636
03637
03638
03639
03640
03641
03642
03643
03644
03645
03646
03647
03648
03649
03650
03651
03652
03653
03654
03655
03656
03657
03658
03659
03660
03661
03662
03663
03664
03665
03666
03667
03668
03669
03670
03671
03672
03673
03674
03675
03676
03677
03678
03679
03680
03681
03682
03683
03684
03685
03686
03687
03688
03689
03690
03691
03692
03693
03694
03695
03696
03697
03698
03699
03700
03701
03702
03703
03704
03705
03706
03707
03708
03709
03710
03711
03712
03713
03714
03715
03716
03717
03718
03719
03720
03721
03722
03723
03724
03725
03726
03727
03728
03729
03730
03731
03732
03733
03734
03735
03736
03737
03738
03739
03740
03741
03742
03743
03744
03745
03746
03747
03748
03749
03750
03751
03752
03753
03754
03755
03756
03757
03758
03759
03760
03761
03762
03763
03764
03765
03766
03767
03768
03769
03770
03771
03772
03773
03774
03775
03776
03777
03778
03779
03780
03781
03782
03783
03784
03785
03786
03787
03788
03789
03790
03791
03792
03793
03794
03795
03796
03797
03798
03799
03800
03801
03802
03803
03804
03805
03806
03807
03808
03809
03810
03811
03812
03813
03814
03815
03816
03817
03818
03819
03820
03821
03822
03823
03824
03825
03826
03827
03828
03829
03830
03831
03832
03833
03834
03835
03836
03837
03838
03839
03840
03841
03842
03843
03844
03845
03846
03847
03848
03849
03850
03851
03852
03853
03854
03855
03856
03857
03858
03859
03860
03861
03862
03863
03864
03865
03866
03867
03868
03869
03870
03871
03872
03873
03874
03875
03876
03877
03878
03879
03880
03881
03882
03883
03884
03885
03886
03887
03888
03889
03890
03891
03892
03893
03894
03895
03896
03897
03898
03899
03900
03901
03902
03903
03904
03905
03906
03907
03908
03909
03910
03911
03912
03913
03914
03915
03916
03917
03918
03919
03920
03921
03922
03923
03924
03925
03926
03927
03928
03929
03930
03931
03932
03933
03934
03935
03936
03937
03938
03939
03940
03941
03942
03943
03944
03945
03946
03947
03948
03949
03950
03951
03952
03953
03954
03955
03956
03957
03958
03959
03960
03961
03962
03963
03964
03965
03966
03967
03968
03969
03970
03971
03972
03973
03974
03975
03976
03977
03978
03979
03980
03981
03982
03983
03984
03985
03986
03987
03988
03989
03990
03991
03992
03993
03994
03995
03996
03997
03998
03999
04000
04001
04002
04003
04004
04005
04006
04007
04008
04009
04010
04011
04012
04013
04014
04015
04016
04017
04018
04019
04020
04021
04022
04023
04024
04025
04026
04027
04028
04029
04030
04031
04032
04033
04034
04035
04036
04037
04038
04039
04040
04041
04042
04043
04044
04045
04046
04047
04048
04049
04050
04051
04052
04053
04054
04055
04056
04057
04058
04059
04060
04061
04062
04063
04064
04065
04066
04067
04068
04069
04070
04071
04072
04073
04074
04075
04076
04077
04078
04079
04080
04081
04082
04083
04084
04085
04086
04087
04088
04089
04090
04091
04092
04093
04094
04095
04096
04097
04098
04099
04100
04101
04102
04103
04104
04105
04106
04107
04108
04109
04110
04111
04112
04113
04114
04115
04116
04117
04118
04119
04120
04121
04122
04123
04124
04125
04126
04127
04128
04129
04130
04131
04132
04133
04134
04135
04136
04137
04138
04139
04140
04141
04142
04143
04144
04145
04146
04147
04148
04149
04150
04151
04152
04153
04154
04155
04156
04157
04158
04159
04160
04161
04162
04163
04164
04165
04166
04167
04168
04169
04170
04171
04172
04173
04174
04175
04176
04177
04178
04179
04180
04181
04182
04183
04184
04185
04186
04187
04188
04189
04190
04191
04192
04193
04194
04195
04196
04197
04198
04199
04200
04201
04202
04203
04204
04205
04206
04207
04208
04209
04210
04211
04212
04213
04214
04215
04216
04217
04218
04219
04220
04221
04222
04223
04224
04225
04226
04227
04228
04229
04230
04231
04232
04233
04234
04235
04236
04237
04238
04239
04240
04241
04242
04243
04244
04245
04246
04247
04248
04249
04250
04251
04252
04253
04254
04255
04256
04257
04258
04259
04260
04261
04262
04263
04264
04265
04266
04267
04268
04269
04270
04271
04272
04273
04274
04275
04276
04277
04278
04279
04280
04281
04282
04283
04284
04285
04286
04287
04288
04289
04290
04291
04292
04293
04294
04295
04296
04297
04298
04299
04300
04301
04302
04303
04304
04305
04306
04307
04308
04309
04310
04311
04312
04313
04314
04315
04316
04317
04318
04319
04320
04321
04322
04323
04324
04325
04326
04327
04328
04329
04330