00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 import roslib; roslib.load_manifest('trf_learn')
00029 import rospy
00030 import functools as ft
00031 import numpy as np
00032 import math
00033 import time
00034 import cv
00035 import tf
00036 import pdb
00037
00038 import tf.transformations as tr
00039 import hrl_camera.ros_camera as rc
00040 import hrl_lib.rutils as ru
00041 import hrl_lib.tf_utils as tfu
00042 import hrl_lib.util as ut
00043 import hrl_opencv.image3d as i3d
00044 import hrl_pr2_lib.pr2 as pr2
00045 import hrl_pr2_lib.linear_move as lm
00046 import hrl_pr2_lib.linear_move as lm
00047 import dynamic_reconfigure.client as dr
00048 import hrl_pr2_lib.devices as hd
00049
00050 def image_diff_val2(before_frame, after_frame):
00051 br = np.asarray(before_frame)
00052 ar = np.asarray(after_frame)
00053 max_sum = br.shape[0] * br.shape[1] * br.shape[2] * 255.
00054 sdiff = np.abs((np.sum(br) / max_sum) - (np.sum(ar) / max_sum))
00055
00056 return sdiff
00057
00058 class ManipulationBehaviors:
00059
00060 def __init__(self, arm, pr2_obj, tf_listener=None):
00061 try:
00062 rospy.init_node('linear_move', anonymous=True)
00063 except Exception, e:
00064 rospy.loginfo('call to init_node failed %s' % str(e))
00065 self.movement = lm.LinearReactiveMovement(arm, pr2_obj, tf_listener)
00066
00067 def reach(self, point, pressure_thres,\
00068 reach_direction=np.matrix([0,0,0]).T, orientation=None):
00069 MOVEMENT_TOLERANCE = .1
00070
00071
00072
00073
00074 self.movement.set_pressure_threshold(pressure_thres)
00075 loc_bl = self.movement.arm_obj.pose_cartesian_tf()[0]
00076 front_loc = point.copy()
00077 front_loc[0,0] = max(loc_bl[0,0], .4)
00078
00079
00080
00081 if orientation == None:
00082 start_loc = self.movement.arm_obj.pose_cartesian_tf()
00083 orientation = start_loc[1]
00084 self.movement.pressure_listener.rezero()
00085
00086
00087 r1, residual_error = self.movement.move_absolute((front_loc, orientation), stop='pressure', pressure=pressure_thres)
00088 rospy.sleep(.1)
00089 r1, residual_error = self.movement.move_absolute((front_loc, orientation), stop='pressure', pressure=pressure_thres)
00090 rospy.sleep(.1)
00091 r1, residual_error = self.movement.move_absolute((front_loc, orientation), stop='pressure', pressure=pressure_thres)
00092
00093
00094
00095
00096
00097
00098
00099 pos_error = None
00100 try:
00101
00102
00103
00104
00105 point_reach = point + reach_direction
00106 r2, pos_error = self.movement.move_absolute((point_reach, \
00107 self.movement.arm_obj.pose_cartesian_tf()[1]), stop='pressure_accel', pressure=pressure_thres)
00108
00109 except lm.RobotSafetyError, e:
00110 rospy.loginfo('robot safety error %s' % str(e))
00111 r2 = None
00112
00113 touch_loc_bl = self.movement.arm_obj.pose_cartesian_tf()
00114
00115 if r2 == 'pressure' or r2 == 'accel' or (pos_error != None and (pos_error < (MOVEMENT_TOLERANCE + np.linalg.norm(reach_direction)))):
00116 self.movement.pressure_listener.rezero()
00117
00118
00119
00120
00121 cur_pose, cur_ang = self.movement.arm_obj.pose_cartesian_tf()
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 return True, r2, touch_loc_bl
00136 else:
00137
00138
00139 return False, r2, None
00140
00141 def press(self, direction, press_pressure, contact_pressure):
00142
00143 self.movement.set_movement_mode_cart()
00144
00145 r1, diff_1 = self.movement.move_relative_gripper(direction, stop='pressure', pressure=contact_pressure)
00146
00147 if r1 == 'pressure' or r1 == 'accel':
00148 self.movement.set_movement_mode_cart()
00149 r2, diff_2 = self.movement.move_relative_gripper(direction, stop='pressure_accel', pressure=press_pressure)
00150 if r2 == 'pressure' or r2 == 'accel' or r2 == None:
00151 return True, r2
00152 else:
00153 return False, r2
00154 else:
00155 return False, r1
00156
00157 class ApplicationBehaviorsDB:
00158
00159 def __init__(self, optical_frame, tf_listener=None):
00160 if tf_listener == None:
00161 tf_listener = tf.TransformListener()
00162 self.tf_listener = tf_listener
00163 self.optical_frame = optical_frame
00164
00165 self.robot = pr2.PR2(self.tf_listener, base=True)
00166 self.behaviors = ManipulationBehaviors('l', self.robot, tf_listener=self.tf_listener)
00167 self.laser_scan = hd.LaserScanner('point_cloud_srv')
00168 self.prosilica = rc.Prosilica('prosilica', 'polled')
00169
00170 self.wide_angle_camera_left = rc.ROSCamera('/wide_stereo/left/image_rect_color')
00171 self.wide_angle_configure = dr.Client('wide_stereo_both')
00172
00173
00174
00175 self.start_location_light_switch = (np.matrix([0.35, 0.30, 1.1]).T, np.matrix([0., 0., 0., 0.1]))
00176 self.start_location_drawer = (np.matrix([0.20, 0.40, .8]).T,
00177 np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
00178 self.folded_pose = np.matrix([ 0.10134791, -0.29295995, 0.41193769]).T
00179 self.driving_param = {'light_switch_up': {'coarse': .9, 'fine': .6, 'voi': .4},
00180 'light_switch_down': {'coarse': .9, 'fine': .6, 'voi': .4},
00181
00182 'light_rocker_down': {'coarse': .9, 'fine': .6, 'voi': .4},
00183 'light_rocker_up': {'coarse': .9, 'fine': .6, 'voi': .4},
00184
00185 'pull_drawer': {'coarse': .9, 'fine': .5, 'voi': .4},
00186 'push_drawer': {'coarse': .9, 'fine': .5, 'voi': .4}}
00187
00188 self.create_arm_poses()
00189 self.driving_posture('light_switch_down')
00190 self.robot.projector.set(False)
00191
00192
00193
00194
00195 def get_behavior_by_task(self, task_type):
00196 if task_type == 'light_switch_down':
00197 return ft.partial(self.light_switch,
00198
00199 point_offset=np.matrix([0,0, -.08]).T,
00200 press_contact_pressure=300,
00201
00202 press_pressure=6000,
00203 press_distance=np.matrix([0.01,0,-.15]).T,
00204 visual_change_thres=.016)
00205
00206 elif task_type == 'light_switch_up':
00207 return ft.partial(self.light_switch,
00208
00209 point_offset=np.matrix([0,0,.08]).T,
00210 press_contact_pressure=300,
00211
00212 press_pressure=6000,
00213 press_distance=np.matrix([0.01,0,.15]).T,
00214 visual_change_thres=.016)
00215
00216 elif task_type == 'light_rocker_up':
00217 return ft.partial(self.light_rocker_push,
00218 pressure=500,
00219 visual_change_thres=.025, offset=np.matrix([0,0,-.05]).T)
00220
00221 elif task_type == 'light_rocker_down':
00222 return ft.partial(self.light_rocker_push,
00223 pressure=500,
00224 visual_change_thres=.025, offset=np.matrix([0,0,.05]).T)
00225
00226 elif task_type == 'pull_drawer':
00227 return self.drawer
00228
00229 elif task_type == 'push_drawer':
00230 return self.drawer_push
00231
00232 else:
00233 pdb.set_trace()
00234
00235 def manipulation_posture(self, task_type):
00236 self.robot.projector.set(False)
00237 for i in range(3):
00238 self.prosilica.get_frame()
00239 self.robot.projector.set(True)
00240
00241
00242 self.robot.left_gripper.open(False, .005)
00243
00244 self.behaviors.movement.pressure_listener.rezero()
00245
00246 if task_type == 'light_switch_down' or task_type == 'light_switch_up':
00247 if np.linalg.norm(self.start_location_light_switch[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
00248 return
00249 self.robot.torso.set_pose(.2, True)
00250 self.untuck()
00251 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00252 self.behaviors.movement.pressure_listener.rezero()
00253
00254 elif task_type == 'light_rocker_up' or task_type == 'light_rocker_down':
00255 if np.linalg.norm(self.start_location_light_switch[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
00256 return
00257 self.robot.torso.set_pose(.2, True)
00258 self.untuck()
00259 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00260 self.behaviors.movement.pressure_listener.rezero()
00261
00262 elif task_type == 'pull_drawer' or task_type == 'push_drawer':
00263 if np.linalg.norm(self.start_location_drawer[0] - self.robot.left.pose_cartesian_tf()[0]) < .3:
00264 return
00265 self.robot.torso.set_pose(0.01, True)
00266 self.untuck()
00267 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='pressure')
00268 self.behaviors.movement.pressure_listener.rezero()
00269 else:
00270 pdb.set_trace()
00271
00272 def driving_posture(self, task_type):
00273 self.robot.projector.set(False)
00274 self.close_gripper()
00275
00276 if np.linalg.norm(self.folded_pose - self.robot.left.pose_cartesian_tf()[0]) < .1:
00277 return
00278
00279 self.robot.torso.set_pose(0.03, True)
00280 self.robot.left_gripper.open(False, .005)
00281 self.robot.right_gripper.open(True, .005)
00282 self.behaviors.movement.pressure_listener.rezero()
00283
00284 if task_type == 'light_switch_down' or task_type == 'light_switch_up':
00285 self.tuck()
00286
00287 elif task_type == 'light_rocker_up' or task_type == 'light_rocker_down':
00288 self.tuck()
00289
00290 elif task_type == 'pull_drawer' or task_type == 'push_drawer':
00291 self.tuck()
00292 else:
00293 pdb.set_trace()
00294
00295
00296
00297
00298 def create_arm_poses(self):
00299 self.right_tucked = np.matrix([[-0.02362532, 1.10477102, -1.55669475, \
00300 -2.12282706, -1.41751231, -1.84175899, 0.21436806]]).T
00301
00302 self.left_tucked = np.matrix([[ 0.05971848, 1.24980184, 1.79045674, \
00303 -1.68333801, -1.73430635, -0.09838841, -0.08641928]]).T
00304
00305
00306 self.r0 = np.matrix([[-0.22774141, 0.7735819 , -1.45102092, \
00307 -2.12152412, -1.14684579, -1.84850287, 0.21397648]]).T
00308
00309
00310 self.l0 = np.matrix([[ 0.06021592, 1.24844832, 1.78901355, -1.68333801, 1.2, -0.10152105, -0.08641928]]).T
00311
00312
00313 self.l1 = np.matrix([[0.94524406, 1.24726399, 1.78548574, -1.79148173, 1.20027637, -1.0, -0.08633226]]).T
00314
00315
00316 self.l2 = np.matrix([[ 1.53180837, 1.24362641, 1.78452361, -1.78829678, 1.1996979,-1.00446167, -0.08741998]]).T
00317
00318 def untuck(self):
00319 if np.linalg.norm(self.robot.left.pose() - self.left_tucked) < .3:
00320 rospy.loginfo('untuck: not in tucked position. Ignoring request')
00321 return
00322
00323 self.behaviors.movement.set_movement_mode_ik()
00324 self.robot.right.set_pose(self.r0, 1.)
00325 self.robot.left.set_poses(np.column_stack([self.l0, self.l1, self.l2]), \
00326 np.array([1., 2., 3.]))
00327 self.robot.right.set_pose(self.right_tucked, 1.)
00328 self.behaviors.movement.set_movement_mode_cart()
00329
00330 def tuck(self):
00331 if np.linalg.norm(self.robot.left.pose() - self.left_tucked) < .5:
00332 rospy.loginfo('tuck: Already tucked. Ignoring request.')
00333 return
00334
00335 self.behaviors.movement.set_movement_mode_ik()
00336 self.robot.right.set_pose(self.r0, 1.)
00337 self.robot.left.set_poses(np.column_stack([self.l2, self.l1, self.l0, self.left_tucked]), \
00338 np.array([4., 5., 6., 7.]))
00339 self.robot.right.set_pose(self.right_tucked, 1.)
00340 self.behaviors.movement.set_movement_mode_cart()
00341
00342 def close_gripper(self):
00343 GRIPPER_CLOSE = .003
00344
00345 self.behaviors.movement.gripper_close()
00346
00347 def open_gripper(self):
00348 GRIPPER_OPEN = .08
00349
00350 self.behaviors.movement.gripper_open()
00351
00352 def look_at(self, point_bl, block=True):
00353
00354
00355 print 'LOOKING AT POINT', point_bl.T, self.optical_frame
00356
00357 self.robot.head.look_at(point_bl, wait=block)
00358
00359
00360
00361
00362
00363
00364 def drive_approach_behavior(self, point_bl, dist_far):
00365
00366 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
00367 point_map = tfu.transform_points(map_T_base_link, point_bl)
00368 t_current_map, r_current_map = self.robot.base.get_pose()
00369 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))
00370
00371 point_dist = np.linalg.norm(point_bl)
00372 bounded_dist = np.max(point_dist - dist_far, 0)
00373 point_close_bl = (point_bl / point_dist) * bounded_dist
00374 point_close_map = tfu.transform_points(map_T_base_link, point_close_bl)
00375 rvalue = self.robot.base.set_pose(point_close_map.T.A1.tolist(), \
00376 r_current_map, '/map', block=True)
00377 t_end, r_end = self.robot.base.get_pose()
00378 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))
00379 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))
00380 rospy.loginfo('drive_approach_behavior: returned %d' % rvalue)
00381 return rvalue
00382
00383
00384
00385
00386 def approach_perpendicular_to_surface(self, point_bl, voi_radius, dist_approach):
00387 map_T_base_link0 = tfu.transform('map', 'base_link', self.tf_listener)
00388 point_map0 = tfu.transform_points(map_T_base_link0, point_bl)
00389
00390 self.turn_to_point(point_bl, block=False)
00391
00392 point_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), \
00393 point_map0)
00394 point_cloud_bl = self.laser_scan.scan(math.radians(180.), math.radians(-180.), 2.5)
00395 point_cloud_np_bl = ru.pointcloud_to_np(point_cloud_bl)
00396 rospy.loginfo('approach_perpendicular_to_surface: pointcloud size %d' \
00397 % point_cloud_np_bl.shape[1])
00398 voi_points_bl, limits_bl = i3d.select_rect(point_bl, voi_radius, voi_radius, voi_radius, point_cloud_np_bl)
00399
00400 normal_bl = i3d.calc_normal(voi_points_bl)
00401 point_in_front_mechanism_bl = point_bl + normal_bl * dist_approach
00402 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
00403 point_in_front_mechanism_map = tfu.transform_points(map_T_base_link, point_in_front_mechanism_bl)
00404
00405
00406 point_map = tfu.transform_points(map_T_base_link, point_bl)
00407 t_current_map, r_current_map = self.robot.base.get_pose()
00408 rospy.loginfo('approach_perpendicular_to_surface: driving for %.3f m to front of surface' \
00409 % np.linalg.norm(t_current_map[0:2] - point_in_front_mechanism_map[0:2,0].T))
00410
00411 rvalue = self.robot.base.set_pose(point_in_front_mechanism_map.T.A1.tolist(), r_current_map, 'map')
00412 if rvalue != 3:
00413 return rvalue
00414
00415 t1_current_map, r1_current_map = self.robot.base.get_pose()
00416 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))
00417
00418
00419 base_link_T_map = tfu.transform('base_link', 'map', self.tf_listener)
00420 point_bl = tfu.transform_points(base_link_T_map, point_map)
00421
00422 self.turn_to_point(point_bl, block=False)
00423 time.sleep(2.)
00424
00425 return rvalue
00426
00427 def approach_location(self, point_bl, coarse_stop, fine_stop, voi_radius=.2):
00428 point_dist = np.linalg.norm(point_bl[0:2,0])
00429 rospy.loginfo('approach_location: Point is %.3f away.' % point_dist)
00430 map_T_base_link = tfu.transform('map', 'base_link', self.tf_listener)
00431 point_map = tfu.transform_points(map_T_base_link, point_bl)
00432
00433 dist_theshold = coarse_stop + .1
00434 if point_dist > dist_theshold:
00435 rospy.loginfo('approach_location: Point is greater than %.1f m away (%.3f). Driving closer.' % (dist_theshold, point_dist))
00436 rospy.loginfo('approach_location: point_bl ' + str(point_bl.T))
00437
00438 ret = self.drive_approach_behavior(point_bl, dist_far=coarse_stop)
00439 base_link_T_map = tfu.transform('base_link', 'map', self.tf_listener)
00440 point_bl_t1 = tfu.transform_points(base_link_T_map, point_map)
00441 if ret != 3:
00442 dist_end = np.linalg.norm(point_bl_t1[0:2,0])
00443 if dist_end > dist_theshold:
00444 rospy.logerr('approach_location: drive_approach_behavior failed! %.3f' % dist_end)
00445 self.robot.sound.say("I am unable to navigate to that location")
00446 return False, 'failed'
00447
00448 ret = self.approach_perpendicular_to_surface(point_bl_t1, voi_radius=voi_radius, dist_approach=fine_stop)
00449 if ret != 3:
00450 rospy.logerr('approach_location: approach_perpendicular_to_surface failed!')
00451 return False, 'failed'
00452
00453 self.robot.sound.say('done')
00454 rospy.loginfo('approach_location: DONE DRIVING!')
00455 return True, 'done'
00456 else:
00457 return False, 'ignored'
00458
00459 def turn_to_point(self, point_bl, block=True):
00460 ang = math.atan2(point_bl[1,0], point_bl[0,0])
00461 rospy.loginfo('turn_to_point: turning by %.2f deg' % math.degrees(ang))
00462
00463 self.robot.base.turn_by(-ang, block=block, overturn=True)
00464
00465 def location_approach_driving(self, task, point_bl):
00466
00467 ap_result = self.approach_location(point_bl,
00468 coarse_stop=self.driving_param[task]['coarse'],
00469 fine_stop=self.driving_param[task]['fine'],
00470 voi_radius=self.driving_param[task]['voi'])
00471
00472 if ap_result[1] == 'failed':
00473 return False, 'approach_location failed'
00474
00475 if ap_result[1] == 'ignore':
00476
00477 ret = self.approach_perpendicular_to_surface(point_bl,
00478 voi_radius=self.driving_param[task]['voi'],
00479 dist_approach=self.driving_param[task]['fine'])
00480 if ret != 3:
00481 rospy.logerr('location_approach_driving: approach_perpendicular_to_surface failed!')
00482 return False, 'approach_perpendicular_to_surface failed'
00483 else:
00484 return True, None
00485
00486 return True, None
00487
00488 def move_base_planner(self, trans, rot):
00489
00490 p_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), np.matrix(trans).T)
00491
00492 self.turn_to_point(p_bl)
00493 rvalue = self.robot.base.set_pose(trans, rot, '/map', block=True)
00494 p_bl = tfu.transform_points(tfu.transform('base_link', 'map', self.tf_listener), np.matrix(trans).T)
00495
00496 self.robot.base.move_to(p_bl[0:2,0], True)
00497 t_end, r_end = self.robot.base.get_pose()
00498 return rvalue==3, np.linalg.norm(t_end[0:2] - np.array(trans)[0:2])
00499
00500
00501
00502
00503
00504 def camera_change_detect(self, threshold, f, args):
00505 config = self.wide_angle_configure.get_configuration()
00506 config['auto_gain'] = False
00507 config['auto_exposure'] = False
00508 self.wide_angle_configure.update_configuration(config)
00509
00510
00511 start_pose = self.robot.head.pose()
00512
00513
00514 self.robot.head.set_pose(np.radians(np.matrix([-30., -30]).T), 1)
00515 time.sleep(4)
00516 for i in range(7):
00517 before_frame = self.wide_angle_camera_left.get_frame()
00518 cv.SaveImage('before.png', before_frame)
00519 f_return = f(*args)
00520 time.sleep(5)
00521 for i in range(7):
00522 after_frame = self.wide_angle_camera_left.get_frame()
00523
00524 cv.SaveImage('after.png', after_frame)
00525 sdiff = image_diff_val2(before_frame, after_frame)
00526 self.robot.head.set_pose(start_pose, 1)
00527 self.robot.head.set_pose(start_pose, 1)
00528 time.sleep(3)
00529
00530
00531 config['auto_gain'] = True
00532 config['auto_exposure'] = True
00533 self.wide_angle_configure.update_configuration(config)
00534
00535 rospy.loginfo('=======================================')
00536 rospy.loginfo('=======================================')
00537 rospy.loginfo('camera difference %.4f (thres %.3f)' % (sdiff, threshold))
00538 if sdiff > threshold:
00539 rospy.loginfo('difference detected!')
00540 return True, f_return
00541 else:
00542 rospy.loginfo('NO differences detected!')
00543 return False, f_return
00544 rospy.loginfo('=======================================')
00545 rospy.loginfo('=======================================')
00546
00547 def light_switch(self, point,
00548 point_offset, press_contact_pressure,
00549 press_pressure, press_distance, visual_change_thres):
00550
00551 try:
00552
00553
00554
00555 rospy.loginfo('reaching to ' + str(point.T))
00556
00557
00558 self.close_gripper()
00559
00560 time.sleep(1)
00561 self.behaviors.movement.pressure_listener.rezero()
00562
00563
00564
00565
00566
00567 success, reason, touchloc_bl = self.behaviors.reach(point, \
00568 press_contact_pressure, \
00569 reach_direction=np.matrix([0.1,0,0]).T)
00570
00571
00572
00573 if touchloc_bl != None:
00574 dist = np.linalg.norm(point - touchloc_bl[0])
00575
00576
00577
00578 rospy.loginfo('Touched point is %.3f m away from observed point' % dist)
00579
00580
00581
00582 if not success:
00583 error_msg = 'Reach failed due to "%s"' % reason
00584 rospy.loginfo(error_msg)
00585 rospy.loginfo('Failure recovery: moving back')
00586 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='accel', \
00587 pressure=press_contact_pressure)
00588
00589 return False, None, point+point_offset
00590
00591 rospy.loginfo('pressing')
00592
00593
00594 self.behaviors.movement.pressure_listener.rezero()
00595 change, press_ret = self.camera_change_detect(visual_change_thres, \
00596 self.behaviors.press, \
00597 (press_distance, press_pressure, press_contact_pressure))
00598 success, reason = press_ret
00599 if not success:
00600 rospy.loginfo('Press failed due to "%s"' % reason)
00601
00602
00603
00604 rospy.loginfo('moving back')
00605
00606 r1, pos_error1 = self.behaviors.movement.move_relative_gripper(np.matrix([-.03, 0., 0.]).T, \
00607 stop='none', pressure=press_contact_pressure)
00608 if r1 != None:
00609 rospy.loginfo('moving back failed due to "%s"' % r1)
00610 return change, None, point+point_offset
00611
00612 rospy.loginfo('reseting')
00613 self.behaviors.movement.pressure_listener.rezero()
00614 r2, pos_error2 = self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00615 if r2 != None and r2 != 'no solution':
00616 rospy.loginfo('moving back to start location failed due to "%s"' % r2)
00617 return change, None, point+point_offset
00618 self.behaviors.movement.pressure_listener.rezero()
00619
00620 rospy.loginfo('DONE.')
00621 return change, touchloc_bl, point+point_offset
00622
00623 except lm.RobotSafetyError, e:
00624 rospy.loginfo('>>>> ROBOT SAFETY ERROR! RESETTING. %s' % str(e))
00625 self.behaviors.movement.pressure_listener.rezero()
00626 r2, pos_error2 = self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure')
00627 return change, None, point+point_offset
00628
00629 def light_rocker_push(self, point, pressure, visual_change_thres, offset):
00630 rospy.loginfo('Reaching')
00631 linear_movement = self.behaviors.movement
00632
00633 self.close_gripper()
00634 self.behaviors.movement.pressure_listener.rezero()
00635
00636
00637 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00638 rospy.sleep(.1)
00639 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00640 rospy.sleep(.1)
00641 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00642 start_dist_error = np.linalg.norm(self.start_location_light_switch[0] - self.robot.left.pose_cartesian_tf()[0])
00643 print '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
00644 print '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
00645 print 'dist error', start_dist_error
00646 print '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
00647 print '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
00648 if not ((start_dist_error > .1) and (start_dist_error < .15)):
00649 pdb.set_trace()
00650
00651
00652
00653 def reach_with_back_up(point, thres, reach_direction):
00654 self.behaviors.reach(point, thres, reach_direction)
00655 try:
00656 r1, pos_error1 = self.behaviors.movement.move_relative_gripper(np.matrix([-.05, 0., 0.]).T, stop='none')
00657 except lm.RobotSafetyError, e:
00658 rospy.loginfo('robot safety error %s' % str(e))
00659 change, press_ret = self.camera_change_detect(visual_change_thres, \
00660
00661 reach_with_back_up, \
00662
00663 (point, pressure, np.matrix([.1,0,0]).T))
00664 try:
00665 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel')
00666 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00667 except lm.RobotSafetyError, e:
00668 rospy.loginfo('robot safety error %s' % str(e))
00669
00670 try:
00671 self.behaviors.movement.move_absolute(self.start_location_light_switch, stop='pressure_accel', pressure=3000)
00672 except lm.RobotSafetyError, e:
00673 rospy.loginfo('robot safety error %s' % str(e))
00674
00675 rospy.loginfo('Reseting')
00676 return change, '', point+offset
00677
00678 def drawer_push(self, point_bl):
00679 PUSH_TOLERANCE = .1
00680
00681 linear_movement = self.behaviors.movement
00682
00683
00684 self.open_gripper()
00685 self.behaviors.movement.pressure_listener.rezero()
00686
00687 rospy.loginfo("Moving to start location")
00688
00689
00690 linear_movement.move_absolute((self.start_location_drawer[0],
00691
00692 np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))),
00693 stop='pressure_accel', pressure=1000)
00694
00695
00696 self.behaviors.movement.set_pressure_threshold(1000)
00697 loc_bl = self.behaviors.movement.arm_obj.pose_cartesian_tf()[0]
00698 front_loc = point_bl.copy()
00699 front_loc[0,0] = max(loc_bl[0,0], .4)
00700
00701
00702
00703 rospy.loginfo("Moving to front location")
00704
00705 orientation = np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))
00706 self.behaviors.movement.pressure_listener.rezero()
00707 r1, residual_error = self.behaviors.movement.move_absolute((front_loc, orientation),
00708 stop='pressure', pressure=1500)
00709 linear_movement.pressure_listener.rezero()
00710
00711
00712 rospy.loginfo("Touching surface")
00713 try:
00714 linear_movement.move_relative_gripper(np.matrix([.5,0,0]).T, stop='pressure_accel', pressure=100)
00715 except lm.RobotSafetyError, e:
00716 rospy.loginfo('robot safety error %s' % str(e))
00717 contact_loc_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
00718
00719
00720 rospy.loginfo("PUSH!!!")
00721 current_position = self.robot.left.pose_cartesian_tf()
00722 target_position = current_position[0] + np.matrix([.4,0,0.]).T
00723 try:
00724
00725 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00726 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00727 linear_movement.move_absolute((target_position, current_position[1]), stop='pressure_accel', pressure=6000)
00728 except lm.RobotSafetyError, e:
00729 rospy.loginfo('robot safety error %s' % str(e))
00730
00731 pushed_loc_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
00732
00733 rospy.loginfo("Moving away")
00734 try:
00735 linear_movement.move_relative_gripper(np.matrix([-.05,0,0]).T, stop='accel')
00736 except lm.RobotSafetyError, e:
00737 rospy.loginfo('robot safety error %s' % str(e))
00738 try:
00739 linear_movement.move_relative_gripper(np.matrix([-.10,0,0]).T, stop='accel')
00740 except lm.RobotSafetyError, e:
00741 rospy.loginfo('robot safety error %s' % str(e))
00742 try:
00743 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel')
00744 except lm.RobotSafetyError, e:
00745 rospy.loginfo('robot safety error %s' % str(e))
00746
00747 linear_movement.pressure_listener.rezero()
00748
00749 linear_movement.move_absolute((self.start_location_drawer[0],
00750
00751 np.matrix(tr.quaternion_from_euler(np.radians(0.), 0, 0))),
00752 stop='pressure_accel', pressure=1000)
00753
00754 move_dist = np.linalg.norm(contact_loc_bl - pushed_loc_bl)
00755 rospy.loginfo('pushed for distance %.3f' % move_dist)
00756 success = move_dist > PUSH_TOLERANCE
00757 return success, 'pushed', pushed_loc_bl
00758
00759 def drawer(self, point):
00760
00761 GRIPPER_OPEN = .08
00762 GRIPPER_CLOSE = .003
00763 MAX_HANDLE_SIZE = .03
00764 linear_movement = self.behaviors.movement
00765 gripper = self.robot.left_gripper
00766
00767
00768
00769 self.open_gripper()
00770 linear_movement.move_absolute((self.start_location_drawer[0],
00771
00772 np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0))),
00773 stop='pressure_accel', pressure=1000)
00774
00775
00776 success, reason, touchloc_bl = self.behaviors.reach(point, 300,
00777 reach_direction=np.matrix([0.1, 0, 0]).T,
00778 orientation=np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
00779
00780
00781 if not success:
00782 error_msg = 'Reach failed due to "%s"' % reason
00783 rospy.loginfo(error_msg)
00784 rospy.loginfo('Failure recovery: moving back')
00785 try:
00786 linear_movement.move_relative_gripper(np.matrix([-.25,0,0]).T, stop='pressure_accel', pressure=300)
00787 except lm.RobotSafetyError, e:
00788 rospy.loginfo('robot safety error %s' % str(e))
00789 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='accel', pressure=300)
00790 return False, 'reach failed', point
00791
00792
00793 GRASP_THRES = 100
00794 try:
00795 linear_movement.move_relative_gripper(np.matrix([-.01,0,0]).T, stop='none')
00796 except lm.RobotSafetyError, e:
00797 rospy.loginfo('robot safety error %s' % str(e))
00798
00799
00800 self.close_gripper()
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815 grasped_handle = (gripper.pose()[0,0] > GRIPPER_CLOSE) and (gripper.pose()[0,0] < MAX_HANDLE_SIZE)
00816
00817 if not grasped_handle:
00818 rospy.loginfo('Failed to grasp handle :(')
00819
00820 self.open_gripper()
00821
00822 linear_movement.pressure_listener.rezero()
00823 linear_movement.move_relative_gripper(np.matrix([-.25,0,0]).T, stop='pressure_accel', pressure=300)
00824 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='accel', pressure=300)
00825 return False, 'failed to grasp handle', point
00826
00827
00828 linear_movement.pressure_listener.rezero()
00829 linear_movement.move_relative_gripper(np.matrix([-.1,0,0]).T, stop='accel', pressure=2500)
00830 linear_movement.move_absolute(linear_movement.arm_obj.pose_cartesian_tf(),
00831 stop='pressure_accel', pressure=300)
00832
00833
00834 self.close_gripper()
00835 rospy.sleep(1)
00836 linear_movement.pressure_listener.rezero()
00837
00838
00839
00840 still_has_handle = gripper.pose()[0,0] > GRIPPER_CLOSE
00841
00842 try:
00843 linear_movement.move_relative_base(np.matrix([-.15,0,0]).T, stop='accel', pressure=2500)
00844 except lm.RobotSafetyError, e:
00845
00846 self.open_gripper()
00847 linear_movement.pressure_listener.rezero()
00848 rospy.loginfo('robot safety error %s' % str(e))
00849
00850
00851
00852 location_handle_bl = linear_movement.arm_obj.pose_cartesian_tf()[0]
00853
00854
00855 self.open_gripper()
00856 rospy.sleep(2)
00857 linear_movement.pressure_listener.rezero()
00858
00859
00860 linear_movement.move_relative_base(np.matrix([-.2, 0, 0.]).T, stop='pressure_accel', pressure=1000)
00861 linear_movement.move_relative_base(np.matrix([-.1, .2, 0.1]).T, stop='pressure_accel', pressure=1000)
00862 self.behaviors.movement.move_absolute(self.start_location_drawer, stop='pressure_accel', pressure=1000)
00863
00864 return still_has_handle, 'pulled', location_handle_bl
00865