00001 import roslib; roslib.load_manifest('hrl_pr2_lib')
00002 import rospy
00003
00004 import pr2_gripper_reactive_approach.reactive_grasp as rgr
00005 import pr2_gripper_reactive_approach.controller_manager as con
00006 import object_manipulator.convert_functions as cf
00007
00008 from actionlib_msgs.msg import GoalStatus
00009 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
00010 from pr2_gripper_sensor_msgs.msg import PR2GripperEventDetectorGoal
00011
00012
00013
00014 import hrl_lib.tf_utils as tfu
00015 import hrl_pr2_lib.pressure_listener as pm
00016 import numpy as np
00017 import tf
00018 import pdb
00019 import time
00020 import hrl_lib.util as ut
00021 import math
00022
00023
00024 class RobotSafetyError(Exception):
00025 def __init__(self, value):
00026 self.parameter = value
00027
00028 def __str__(self):
00029 return repr(self.parameter)
00030
00031
00032 class ArmStoppedDetector:
00033 def __init__(self, pos_thres=0.0001, rot_thres=0.001, time_settle=1., hz=30., n_step=3000):
00034 self.pos_diff = []
00035 self.rot_diff = []
00036 self.times = []
00037
00038 self.last_pos = None
00039 self.last_t = None
00040 self.n_step = n_step
00041 self.hz = hz
00042 self.time_settle = time_settle
00043
00044 self.pos_thres = pos_thres
00045 self.rot_thres = rot_thres
00046 self.start_time = time.time()
00047
00048 def record_diff(self, loc_mat):
00049 cur_t = time.time() - self.start_time
00050 if self.last_t == None or (cur_t - self.last_t) > (1./self.hz):
00051 pos, rot = tfu.matrix_as_tf(loc_mat)
00052 if self.last_pos != None:
00053 self.pos_diff.append(np.linalg.norm(np.matrix(pos) - np.matrix(self.last_pos[0])))
00054
00055 lp = np.array(self.last_pos[1]) / np.linalg.norm(self.last_pos[1])
00056 r = np.array(rot) / np.linalg.norm(rot)
00057
00058 self.rot_diff.append(np.linalg.norm(lp - r))
00059 self.times.append(cur_t)
00060
00061 sidx = len(self.pos_diff) - self.n_step
00062 self.pos_diff = self.pos_diff[sidx:]
00063 self.pose_diff = self.rot_diff[sidx:]
00064
00065 self.last_pos = tfu.matrix_as_tf(loc_mat)
00066 self.last_t = cur_t
00067
00068 def is_stopped(self):
00069 cur_t = time.time() - self.start_time
00070 pos_over_thres_idx = np.where(np.array(self.pos_diff) > self.pos_thres)[0]
00071 rot_over_thres_idx = np.where(np.array(self.rot_diff) > self.rot_thres)[0]
00072 if len(pos_over_thres_idx) > 0 or len(rot_over_thres_idx) > 0:
00073 max_times = []
00074 if len(pos_over_thres_idx) > 0:
00075 max_times.append(self.times[pos_over_thres_idx[-1]])
00076 if len(rot_over_thres_idx) > 0:
00077 max_times.append(self.times[rot_over_thres_idx[-1]])
00078 tmax = np.max(max_times)
00079 if (cur_t - tmax) > self.time_settle:
00080 print cur_t - tmax, tmax, self.time_settle, max_times
00081 return True
00082 else:
00083 return False
00084 elif len(self.pos_diff) > (self.time_settle * self.hz):
00085 return True
00086 else:
00087 return False
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 class LinearReactiveMovement:
00098
00099
00100
00101
00102
00103 def __init__(self, arm, pr2, tf_listener, using_slip_controller=1, using_slip_detection=1):
00104 if tf_listener == None:
00105 self.tf_listener = tf.TransformListener()
00106 else:
00107 self.tf_listener = tf_listener
00108 self.pr2 = pr2
00109
00110 if arm == 'l':
00111 self.ik_frame = 'l_wrist_roll_link'
00112 self.tool_frame = 'l_gripper_tool_frame'
00113 self.arm_obj = self.pr2.left
00114 ptopic = '/pressure/l_gripper_motor'
00115 else:
00116 self.ik_frame = 'r_wrist_roll_link'
00117 self.tool_frame = 'r_gripper_tool_frame'
00118 self.arm_obj = self.pr2.right
00119 ptopic = '/pressure/r_gripper_motor'
00120
00121 self.pressure_listener = pm.PressureListener(ptopic, 5000)
00122
00123 print 'INITIALIZING CONTROLLER MANAGER'
00124 self.cman = con.ControllerManager(arm, self.tf_listener, using_slip_controller,
00125 using_slip_detection)
00126 print 'INITIALIZAING REACTIVE GRASPER'
00127 self.reactive_gr = rgr.ReactiveGrasper(self.cman)
00128
00129
00130
00131
00132
00133
00134
00135 self.movement_mode = 'cart'
00136
00137
00138 self.timeout = 50.
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153 def move_absolute_old(self, loc, stop='pressure_accel', pressure=300):
00154 self.set_pressure_threshold(pressure)
00155 stop_funcs = self._process_stop_option(stop)
00156 self.set_movement_mode_cart()
00157
00158 self._move_cartesian(loc[0], loc[1], stop_funcs, timeout=self.timeout, settling_time=5.0)
00159 self.set_movement_mode_ik()
00160 r = self._move_cartesian(loc[0], loc[1], stop_funcs, timeout=self.timeout, settling_time=5.0)
00161
00162
00163 diff = loc[0] - self.arm_obj.pose_cartesian_tf()[0]
00164 rospy.loginfo('move_absolute: diff is %s' % str(diff.T))
00165 rospy.loginfo('move_absolute: dist %.3f' % np.linalg.norm(diff))
00166 return r, np.linalg.norm(diff)
00167
00168
00169 def move_absolute(self, loc, stop='pressure_accel', pressure=300, frame='base_link'):
00170 self.set_pressure_threshold(pressure)
00171 stop_funcs = self._process_stop_option(stop)
00172 r = self._move_cartesian(loc[0], loc[1], stop_funcs, timeout=self.timeout,
00173 settling_time=5.0, frame=frame)
00174 diff = loc[0] - self.arm_obj.pose_cartesian_tf()[0]
00175 rospy.loginfo('move_absolute: diff is %s' % str(diff.T))
00176 rospy.loginfo('move_absolute: dist %.3f' % np.linalg.norm(diff))
00177 return r, np.linalg.norm(diff)
00178
00179
00180
00181
00182
00183
00184
00185
00186 def move_relative(self, movement_target, target_frame, stop='pressure_accel', pressure=150):
00187 base_T_target = tfu.transform('base_link', target_frame, self.tf_listener)
00188 movement_base = base_T_target[0:3, 0:3] * movement_target
00189 return self.move_relative_base(movement_base, stop=stop, pressure=pressure)
00190
00191
00192
00193
00194
00195
00196
00197 def move_relative_gripper(self, movement_tool, stop='pressure_accel', pressure=150):
00198 base_T_tool = tfu.transform('base_link', self.tool_frame, self.tf_listener)
00199 movement_base = base_T_tool[0:3, 0:3] * movement_tool
00200 return self.move_relative_base(movement_base, stop=stop, pressure=pressure)
00201
00202
00203
00204
00205
00206
00207
00208 def move_relative_base(self, movement, stop='pressure_accel', pressure=150):
00209 self.set_pressure_threshold(pressure)
00210 trans, rot = self.arm_obj.pose_cartesian_tf()
00211 ntrans = trans + movement
00212 stop_funcs = self._process_stop_option(stop)
00213 r = self._move_cartesian(ntrans, rot, stop_funcs, \
00214 timeout=self.timeout, settling_time=5.0)
00215
00216
00217 diff = self.arm_obj.pose_cartesian_tf()[0] - (trans + movement)
00218 rospy.loginfo('move_relative_base: diff is ' + str(diff.T))
00219 rospy.loginfo('move_relative_base: dist %.3f' % np.linalg.norm(diff))
00220 return r, np.linalg.norm(diff)
00221
00222
00223
00224 def gripper_close(self):
00225 self.reactive_gr.compliant_close()
00226
00227
00228
00229 def gripper_open(self):
00230 self.reactive_gr.cm.command_gripper(.1, -1, 1)
00231
00232 def set_pressure_threshold(self, t):
00233 self.pressure_listener.set_threshold(t)
00234
00235
00236
00237 def set_movement_mode_ik(self):
00238 self.movement_mode = 'ik'
00239 self.reactive_gr.cm.switch_to_joint_mode()
00240 self.reactive_gr.cm.freeze_arm()
00241
00242
00243
00244 def set_movement_mode_cart(self):
00245 self.movement_mode = 'cart'
00246
00247 def _move_cartesian(self, position, orientation, \
00248 stop_funcs=[], timeout = 3.0, settling_time = 0.5, \
00249 frame='base_link', vel=.15):
00250 if self.movement_mode == 'ik':
00251 return self._move_cartesian_ik(position, orientation, stop_funcs, \
00252 timeout, settling_time, frame, vel=.15)
00253 elif self.movement_mode == 'cart':
00254 return self._move_cartesian_cart(position, orientation, stop_funcs, \
00255 timeout, settling_time, frame)
00256
00257
00258
00259
00260
00261 def _move_cartesian_cart(self, position, orientation, \
00262 stop_funcs=[], timeout = 3.0, settling_time = 0.5, frame='base_link'):
00263
00264
00265
00266 self.tf_listener.waitForTransform('base_link', frame, rospy.Time(),
00267 rospy.Duration(10))
00268 frame_T_base = tfu.transform('base_link', frame, self.tf_listener)
00269 init_cart_pose = tfu.tf_as_matrix((position.A1.tolist(),
00270 orientation.A1.tolist()))
00271 base_cart_pose = frame_T_base*init_cart_pose
00272
00273
00274 self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10))
00275 toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener)
00276
00277 base_cart_pose = base_cart_pose * toolframe_T_ikframe
00278 base_position, base_orientation = tfu.matrix_as_tf(base_cart_pose)
00279
00280 pose_stamped = cf.create_pose_stamped(base_position.tolist() + base_orientation.tolist())
00281 rg = self.reactive_gr
00282 rg.check_preempt()
00283
00284
00285
00286 (pos, rot) = cf.pose_stamped_to_lists(rg.cm.tf_listener, pose_stamped, 'base_link')
00287 rg.move_cartesian_step(pos+rot, timeout, settling_time)
00288
00289
00290 start_time = rospy.get_rostime()
00291 done_time = None
00292
00293 stop_trigger = None
00294
00295 stop_detector = ArmStoppedDetector()
00296 while(1):
00297
00298 rg.check_preempt()
00299 if len(stop_funcs) > 0:
00300 for f, name in stop_funcs:
00301 if f():
00302 rg.cm.switch_to_joint_mode()
00303 rg.cm.freeze_arm()
00304
00305 stop_trigger = name
00306 rospy.loginfo('"%s" requested that motion should be stopped.' % (name))
00307 break
00308 if stop_trigger != None:
00309 break
00310
00311
00312
00313
00314
00315
00316
00317
00318 if rg.cm.check_cartesian_near_pose(pose_stamped, .0025, .05):
00319 rospy.loginfo("_move_cartesian_cart: reached pose")
00320
00321 break
00322
00323 stop_detector.record_diff(self.arm_obj.pose_cartesian())
00324 if stop_detector.is_stopped():
00325 rospy.loginfo("_move_cartesian_cart: arm stopped")
00326
00327 break
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349 if stop_trigger == 'pressure_safety':
00350 print 'ROBOT SAFETY ERROR'
00351
00352
00353
00354
00355 return stop_trigger
00356
00357 def _move_cartesian_ik(self, position, orientation, \
00358 stop_funcs=[], timeout = 30., settling_time = 0.25, \
00359 frame='base_link', vel=.15):
00360
00361
00362
00363
00364 self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10))
00365 toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener)
00366 cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist()))
00367 cart_pose = cart_pose * toolframe_T_ikframe
00368 position, orientation = tfu.matrix_as_tf(cart_pose)
00369
00370
00371 goal_pose_ps = cf.create_pose_stamped(position.tolist() + orientation.tolist(), frame)
00372 r = self.reactive_gr.cm.move_cartesian_ik(goal_pose_ps, blocking=0, step_size=.005, \
00373 pos_thres=.02, rot_thres=.1, timeout=rospy.Duration(timeout),
00374 settling_time=rospy.Duration(settling_time), vel=vel)
00375 if not (r == 'sent goal' or r == 'success'):
00376 return r
00377
00378
00379
00380
00381
00382
00383 stop_trigger = None
00384 done_time = None
00385 start_time = rospy.get_rostime()
00386 while stop_trigger == None:
00387 for f, name in stop_funcs:
00388 if f():
00389 self.arm_obj.stop_trajectory_execution()
00390 stop_trigger = name
00391 rospy.loginfo('"%s" requested that motion should be stopped.' % (name))
00392 break
00393
00394 if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout):
00395 rospy.loginfo("_move_cartesian_ik: motion timed out")
00396 break
00397
00398 if (not done_time) and (not self.arm_obj.has_active_goal()):
00399
00400 done_time = rospy.get_rostime()
00401
00402 if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time):
00403 rospy.loginfo("_move_cartesian_ik: done settling")
00404 break
00405
00406 if stop_trigger == 'pressure_safety':
00407 print 'ROBOT SAFETY ERROR'
00408
00409 return stop_trigger
00410
00411 def _check_gripper_event(self):
00412
00413 state = self.cman.get_gripper_event_detector_state()
00414 if state not in [GoalStatus.ACTIVE, GoalStatus.PENDING]:
00415 rospy.loginfo('Gripper event detected.')
00416 return True
00417 else:
00418 return False
00419
00420
00421
00422
00423 def _start_gripper_event_detector(self, event_type = 'all', accel = 8.25, slip=.008, blocking = 0, timeout = 15.):
00424
00425 goal = PR2GripperEventDetectorGoal()
00426 if event_type == 'accel':
00427 goal.command.trigger_conditions = goal.command.ACC
00428 elif event_type == 'slip':
00429 goal.command.trigger_conditions = goal.command.SLIP
00430 elif event_type == 'press_accel':
00431 goal.command.trigger_conditions = goal.command.FINGER_SIDE_IMPACT_OR_ACC
00432 elif event_type == 'slip_accel':
00433 goal.command.trigger_conditions = goal.command.SLIP_AND_ACC
00434 else:
00435 goal.command.trigger_conditions = goal.command.FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC
00436
00437
00438 goal.command.acceleration_trigger_magnitude = accel
00439 goal.command.slip_trigger_magnitude = slip
00440
00441 rospy.loginfo("starting gripper event detector")
00442 self.cman.gripper_event_detector_action_client.send_goal(goal)
00443
00444
00445 if blocking:
00446 finished_within_time = self.cman.gripper_event_detector_action_client.wait_for_result(rospy.Duration(timeout))
00447 if not finished_within_time:
00448 rospy.logerr("Gripper didn't see the desired event trigger before timing out")
00449 return 0
00450 state = self.cman.gripper_event_detector_action_client.get_state()
00451 if state == GoalStatus.SUCCEEDED:
00452 result = self.cman.gripper_event_detector_action_client.get_result()
00453 if result.data.placed:
00454 return 1
00455 return 0
00456
00457 def _process_stop_option(self, stop):
00458 stop_funcs = []
00459 self.pressure_listener.check_safety_threshold()
00460
00461
00462
00463
00464
00465 if stop == 'pressure':
00466 self.pressure_listener.check_threshold()
00467 stop_funcs.append([self.pressure_listener.check_threshold, 'pressure'])
00468
00469 elif stop == 'pressure_accel':
00470
00471
00472 self.pressure_listener.check_threshold()
00473 stop_funcs.append([self.pressure_listener.check_threshold, 'pressure'])
00474
00475 self._start_gripper_event_detector(event_type='accel', timeout=self.timeout)
00476 stop_funcs.append([self._check_gripper_event, 'accel'])
00477
00478 return stop_funcs
00479
00480
00481
00482
00483 if __name__ == '__main__':
00484 mode = 'run'
00485
00486 if mode == 'plot':
00487 import pylab as pb
00488 import sys
00489
00490 a = ut.load_pickle(sys.argv[1])
00491 print len(a.pos_diff)
00492 pb.plot(a.pos_diff)
00493 pb.show()
00494
00495 exit(0)
00496
00497 else:
00498
00499 import hrl_pr2_lib.pr2 as pr2
00500 rospy.init_node('test_linear_move')
00501 arm = 'r'
00502 tflistener = tf.TransformListener()
00503 robot = pr2.PR2(tflistener)
00504 movement = LinearReactiveMovement(arm, robot, tflistener)
00505
00506 if mode == 'save':
00507 poses = []
00508 print 'going.....'
00509 while True:
00510 print 'waiting for input'
00511 r = raw_input()
00512 if r != 's':
00513 print 'getting pose.'
00514 p = movement.arm_obj.pose_cartesian()
00515 print 'pose is', p
00516 poses.append(p)
00517 else:
00518 break
00519
00520 ut.save_pickle(poses, 'saved_poses.pkl')
00521
00522 elif mode == 'run':
00523 poses = ut.load_pickle('saved_poses.pkl')
00524 for p in poses:
00525 print 'hit enter to move'
00526 r = raw_input()
00527 pos, rot = tfu.matrix_as_tf(p)
00528 movement.set_movement_mode_cart()
00529 movement.move_absolute2((np.matrix(pos), np.matrix(rot)))