hrl_face_adls: face_adls_manager.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/python
00002         
00003 import numpy as np
00004 import sys
00005 import cPickle as pkl
00006 from threading import Lock
00007 
00008 import roslib
00009 roslib.load_manifest('hrl_face_adls')
00010 
00011 import rospy
00012 from std_msgs.msg import Int8, String, Bool, Header
00013 from std_srvs.srv import Empty
00014 from geometry_msgs.msg import WrenchStamped, PoseStamped, PoseArray
00015 import rosparam
00016 import roslaunch.substitution_args
00017 from tf import TransformListener, ExtrapolationException, LookupException, ConnectivityException
00018 
00019 from hrl_geom.pose_converter import PoseConv
00020 import hrl_geom.transformations as trans
00021 #from hrl_pr2_arms.pr2_arm_jt_task import create_ep_arm, PR2ArmJTransposeTask
00022 from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher
00023 from hrl_ellipsoidal_control.ellipsoid_space import EllipsoidSpace
00024 from hrl_ellipsoidal_control.ellipsoidal_parameters import *
00025 from hrl_face_adls.face_adls_parameters import *
00026 from hrl_face_adls.msg import StringArray
00027 from hrl_face_adls.srv import EnableFaceController, EnableFaceControllerResponse
00028 from hrl_face_adls.srv import RequestRegistration
00029 
00030 ##
00031 # Returns a function which will call the callback immediately in
00032 # a different thread.
00033 def async_call(cb):
00034     def cb_outer(*args, **kwargs):
00035         def cb_inner(te):
00036             cb(*args, **kwargs)
00037         rospy.Timer(rospy.Duration(0.00001), cb_inner, oneshot=True)
00038     return cb_outer
00039 
00040 class ForceCollisionMonitor(object):
00041     def __init__(self):
00042         self.last_activity_time = rospy.get_time()
00043         self.last_reading = rospy.get_time()
00044         self.last_dangerous_cb_time = 0.0
00045         self.last_timeout_cb_time = 0.0
00046         self.last_contact_cb_time = 0.0
00047         self.in_action = False
00048         self.lock = Lock()
00049         self.dangerous_force_thresh = rospy.get_param("~dangerous_force_thresh", 10.0)
00050         self.activity_force_thresh = rospy.get_param("~activity_force_thresh", 3.0)
00051         self.contact_force_thresh = rospy.get_param("~contact_force_thresh", 3.0)
00052         self.timeout_time = rospy.get_param("~timeout_time", 30.0)
00053         rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, 
00054                          async_call(self.force_cb), queue_size=1)
00055         def check_readings(te):
00056             time_diff = rospy.get_time() - self.last_reading
00057             if time_diff > 3.:
00058                 rospy.logerr("Force monitor hasn't recieved force readings for %.1f seconds!"
00059                               % time_diff)
00060         rospy.Timer(rospy.Duration(3), check_readings)
00061 
00062     def force_cb(self, msg):
00063         self.last_reading = rospy.get_time()
00064         if self.lock.acquire(False):
00065             force_mag = np.linalg.norm([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
00066             if (force_mag > self.dangerous_force_thresh and
00067                     rospy.get_time() - self.last_dangerous_cb_time > DANGEROUS_CB_COOLDOWN):
00068                 self.dangerous_cb(self.dangerous_force_thresh)
00069                 self.last_dangerous_cb_time = rospy.get_time()
00070             elif (force_mag > self.contact_force_thresh and 
00071                     rospy.get_time() - self.last_contact_cb_time > CONTACT_CB_COOLDOWN):
00072                 self.contact_cb(self.contact_force_thresh)
00073                 self.last_contact_cb_time = rospy.get_time()
00074             elif force_mag > self.activity_force_thresh:
00075                 self.update_activity()
00076             elif (self.is_inactive() and 
00077                     rospy.get_time() - self.last_timeout_cb_time > TIMEOUT_CB_COOLDOWN):
00078                 self.timeout_cb(self.timeout_time)
00079                 self.last_timeout_cb_time = rospy.get_time()
00080             self.lock.release()
00081 
00082     def is_inactive(self):
00083         #return not self.in_action and rospy.get_time() - self.last_activity_time > self.timeout_time
00084         return rospy.get_time() - self.last_activity_time > self.timeout_time
00085 
00086     def register_contact_cb(self, cb=lambda x:None):
00087         self.contact_cb = async_call(cb)
00088 
00089     def register_dangerous_cb(self, cb=lambda x:None):
00090         self.dangerous_cb = async_call(cb)
00091 
00092     def register_timeout_cb(self, cb=lambda x:None):
00093         self.timeout_cb = async_call(cb)
00094 
00095     def update_activity(self):
00096         self.last_activity_time = rospy.get_time()
00097 
00098     def start_activity(self):
00099         self.in_action = True
00100 
00101     def stop_activity(self):
00102         self.last_activity_time = rospy.get_time()
00103         self.in_action = False
00104 
00105 class FaceADLsManager(object):
00106     def __init__(self):
00107 
00108         self.ellipsoid = EllipsoidSpace()
00109         self.controller_switcher = ControllerSwitcher()
00110         self.ee_frame = '/l_gripper_shaver45_frame'
00111         self.tfl = TransformListener()
00112         self.is_forced_retreat = False
00113         self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array', PoseArray)
00114 
00115         self.global_input_sub = rospy.Subscriber("/face_adls/global_move", String, 
00116                                                  async_call(self.global_input_cb))
00117         self.local_input_sub = rospy.Subscriber("/face_adls/local_move", String, 
00118                                                 async_call(self.local_input_cb))
00119         self.clicked_input_sub = rospy.Subscriber("/face_adls/clicked_move", PoseStamped, 
00120                                                  async_call(self.global_input_cb))
00121         self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
00122         self.global_move_poses_pub = rospy.Publisher('/face_adls/global_move_poses',
00123                                                      StringArray, latch=True)
00124         self.controller_enabled_pub = rospy.Publisher('/face_adls/controller_enabled',
00125                                                       Bool, latch=True)
00126         self.enable_controller_srv = rospy.Service("/face_adls/enable_controller", 
00127                                                    EnableFaceController, self.enable_controller_cb)
00128         self.request_registration = rospy.ServiceProxy("/request_registration", RequestRegistration)
00129 
00130         self.test_pose = rospy.Publisher('face_adls/test_pose', PoseStamped, latch=True)
00131         self.test_pose1 = rospy.Publisher('face_adls/test_pose1', PoseStamped, latch=True)
00132         self.test_pose2 = rospy.Publisher('face_adls/test_pose2', PoseStamped, latch=True)
00133         self.test_pose3 = rospy.Publisher('face_adls/test_pose3', PoseStamped, latch=True)
00134 
00135         def stop_move_cb(msg):
00136             self.stop_moving()
00137         self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move", Bool, stop_move_cb, queue_size=1)
00138 
00139     def stop_moving(self):
00140         """Send empty PoseArray to skin controller to stop movement"""
00141         self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
00142 
00143     def current_ee_pose(self, link, frame='/torso_lift_link'):
00144         """Get current end effector pose from tf"""
00145 #        print "Getting pose of %s in frame: %s" %(link, frame)
00146         try:
00147             now = rospy.Time.now()
00148             self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
00149             pos, quat = self.tfl.lookupTransform(frame, link, now)
00150         except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
00151             rospy.logwarn("[face_adls_manager] TF Failure getting current end-effector pose: %s" %e)
00152             return None
00153         return pos, quat 
00154 
00155     def publish_feedback(self, message=None):
00156         if message is not None:
00157             rospy.loginfo("[face_adls_manager] %s" % message)
00158             self.feedback_pub.publish(message)
00159 
00160     def enable_controller_cb(self, req):
00161         if req.enable:
00162             face_adls_modes = rospy.get_param('/face_adls_modes', None) 
00163             params = face_adls_modes[req.mode]
00164             self.shaving_side = rospy.get_param('/shaving_side', 'r') # TODO Make more general
00165             self.trim_retreat = req.mode == "shaving"
00166             self.flip_gripper = self.shaving_side == 'r' and req.mode == "shaving"
00167             bounds = params['%s_bounds' % self.shaving_side]
00168             self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
00169             #self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
00170 
00171             reg_resp = self.request_registration(req.mode, self.shaving_side)
00172             if not reg_resp.success:
00173                 self.publish_feedback(Messages.NO_PARAMS_LOADED)
00174                 return EnableFaceControllerResponse(False)
00175 
00176             reg_pose = PoseConv.to_pose_stamped_msg(reg_resp.e_params.e_frame)
00177             try:
00178                 now = rospy.Time.now()
00179                 reg_pose.header.stamp = now
00180                 self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link', 
00181                                           now, rospy.Duration(8.0))
00182                 ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
00183             except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
00184                 rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
00185 
00186             self.ellipsoid.load_ell_params(ellipse_frame_base,
00187                                            reg_resp.e_params.E,
00188                                            reg_resp.e_params.is_oblate,
00189                                            reg_resp.e_params.height)
00190             global_poses_dir = rospy.get_param("~global_poses_dir", "")
00191             global_poses_file = params["%s_ell_poses_file" % self.shaving_side]
00192             global_poses_resolved = roslaunch.substitution_args.resolve_args(
00193                                             global_poses_dir + "/" + global_poses_file)
00194             self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
00195             self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
00196 
00197             #Check rotating gripper based on side of body 
00198             if not self.flip_gripper:
00199                 self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
00200                 print "Rotating gripper by PI around x-axis"
00201             else:
00202                 self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
00203                 print "NOT Rotating gripper around x-axis"
00204 
00205             # check if arm is near head
00206             cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
00207             ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
00208             equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
00209             print ell_pos, equals
00210             if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
00211                 arm_in_bounds =  np.all(equals)
00212             else:
00213                 ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
00214                 min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
00215                 arm_in_bounds = (equals[0] and
00216                         equals[2] and 
00217                         (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
00218 
00219             self.setup_force_monitor()
00220 
00221             success = True
00222             if not arm_in_bounds:
00223                 self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
00224                 success = False
00225 
00226             #Switch back to l_arm_controller if necessary
00227             if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
00228                 print "Controller switch to l_arm_controller succeeded"
00229                 self.publish_feedback(Messages.ENABLE_CONTROLLER)
00230             else:
00231                 print "Controller switch to l_arm_controller failed"
00232                 success = False
00233             self.controller_enabled_pub.publish(Bool(success))
00234         else:
00235             self.stop_moving()
00236             self.controller_enabled_pub.publish(Bool(False))
00237             rospy.loginfo(Messages.DISABLE_CONTROLLER)
00238             success =  True
00239         return EnableFaceControllerResponse(success)
00240 
00241     def setup_force_monitor(self):
00242         self.force_monitor = ForceCollisionMonitor()
00243 
00244         # registering force monitor callbacks
00245         def dangerous_cb(force):
00246             self.stop_moving()
00247             curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
00248             ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
00249             if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
00250                 self.publish_feedback(Messages.DANGEROUS_FORCE %force)
00251                 self.retreat_move(SAFETY_RETREAT_HEIGHT, 
00252                                   SAFETY_RETREAT_VELOCITY,
00253                                   forced=True)
00254         self.force_monitor.register_dangerous_cb(dangerous_cb)
00255 
00256         def timeout_cb(time):
00257             start_time = rospy.get_time()
00258             ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
00259             rospy.loginfo("ELL POS TIME: %.3f " % (rospy.get_time() - start_time) + str(ell_pos) 
00260                           + " times: %.3f %.3f" % (self.force_monitor.last_activity_time, rospy.get_time()))
00261             if ell_pos[2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive():
00262                 self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
00263                 self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
00264         self.force_monitor.register_timeout_cb()
00265 
00266         def contact_cb(force):
00267             self.force_monitor.update_activity()
00268             if not self.is_forced_retreat:
00269                 self.stop_moving()
00270                 self.publish_feedback(Messages.CONTACT_FORCE % force)
00271                 #rospy.loginfo("[face_adls_manZger] Arm stopped due to making contact.")
00272 
00273         self.force_monitor.register_contact_cb()
00274         self.force_monitor.start_activity()
00275         self.force_monitor.update_activity()
00276         self.is_forced_retreat = False
00277 
00278     def retreat_move(self, height, velocity, forced=False):
00279         if forced:
00280             self.is_forced_retreat = True
00281         cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
00282         ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
00283         #print "Retreat EP:", ell_pos
00284         latitude = ell_pos[0]
00285         if self.trim_retreat:
00286             latitude = min(latitude, TRIM_RETREAT_LATITUDE)
00287         #rospy.loginfo("[face_adls_manager] Retreating from current location.")
00288 
00289         retreat_pos = [latitude, ell_pos[1], height]
00290         retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
00291         retreat_quat = [0,0,0,1]
00292         if forced:
00293             cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
00294             cart_msg = [PoseConv.to_pose_msg(cart_path)]
00295         else:
00296             ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
00297                                                               ell_quat,
00298                                                               retreat_pos,
00299                                                               retreat_quat,
00300                                                               velocity=0.01,
00301                                                               min_jerk=True)
00302             cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
00303             cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
00304             
00305         head = Header()
00306         head.frame_id = '/ellipse_frame'
00307         head.stamp = rospy.Time.now()
00308         pose_array = PoseArray(head, cart_msg)
00309         self.pose_traj_pub.publish(pose_array)
00310 
00311         self.is_forced_retreat = False
00312 
00313     def global_input_cb(self, msg):
00314         self.force_monitor.update_activity()
00315         if self.is_forced_retreat:
00316             return
00317         rospy.loginfo("[face_adls_manager] Performing global move.")
00318         if type(msg) == String:
00319             self.publish_feedback(Messages.GLOBAL_START %msg.data)
00320             goal_ell_pose = self.global_poses[msg.data]
00321         elif type(msg) == PoseStamped:
00322             try:
00323                 self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
00324                 goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
00325                 goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
00326                 flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
00327                 goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
00328                 goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
00329                 self.publish_feedback('Moving around ellipse to clicked position).')
00330                 goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
00331             except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
00332                 rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
00333                 return
00334 
00335         curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
00336         curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
00337         retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
00338         retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
00339         approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
00340         #approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
00341         approach_ell_quat = goal_ell_pose[1]
00342         final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
00343         #final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
00344         final_ell_quat = goal_ell_pose[1]
00345         
00346         cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
00347         cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
00348         self.test_pose.publish(cart_ret_msg)
00349 
00350         cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
00351         cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
00352         self.test_pose1.publish(cart_app_msg)
00353 
00354         cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
00355         cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
00356         self.test_pose2.publish(cart_goal_msg)
00357 
00358         retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00359                                                                   retreat_ell_pos, retreat_ell_quat,
00360                                                                   velocity=0.01, min_jerk=True)
00361         transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
00362                                                                      approach_ell_pos, approach_ell_quat,
00363                                                                      velocity=0.01, min_jerk=True)
00364         approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
00365                                                                    final_ell_pos, final_ell_quat,
00366                                                                    velocity=0.01, min_jerk=True)
00367         
00368         full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
00369         full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
00370         cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
00371         head = Header()
00372         head.frame_id = '/ellipse_frame'
00373         head.stamp = rospy.Time.now()
00374         pose_array = PoseArray(head, cart_traj_msg)
00375         self.pose_traj_pub.publish(pose_array)
00376 
00377         ps = PoseStamped()
00378         ps.header = head
00379         ps.pose = cart_traj_msg[0]
00380         self.test_pose3.publish(ps)
00381         
00382         self.force_monitor.update_activity()
00383             
00384     def local_input_cb(self, msg):
00385         self.force_monitor.update_activity()
00386         if self.is_forced_retreat:
00387             return
00388         self.stop_moving()
00389         button_press = msg.data 
00390 
00391         curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
00392         curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
00393 
00394         if button_press in ell_trans_params:
00395             self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
00396             change_trans_ep = ell_trans_params[button_press]
00397             goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
00398                             curr_ell_pos[1]+change_trans_ep[1],
00399                             curr_ell_pos[2]+change_trans_ep[2]]
00400             ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00401                                                               goal_ell_pos, curr_ell_quat,
00402                                                               velocity=ELL_LOCAL_VEL, min_jerk=True)
00403         elif button_press in ell_rot_params:
00404             self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
00405             change_rot_ep = ell_rot_params[button_press]
00406             rot_quat = trans.quaternion_from_euler(*change_rot_ep)
00407             goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
00408             ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00409                                                               curr_ell_pos, goal_ell_quat,
00410                                                               velocity=ELL_ROT_VEL, min_jerk=True)
00411         elif button_press == "reset_rotation":
00412             self.publish_feedback(Messages.ROT_RESET_START)
00413             ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00414                                                               curr_ell_pos,(0.,0.,0.,1.),
00415                                                               velocity=ELL_ROT_VEL, min_jerk=True)
00416         else:
00417             rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
00418 
00419         cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
00420         cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
00421         head = Header()
00422         head.frame_id = '/ellipse_frame'
00423         head.stamp = rospy.Time.now()
00424         pose_array = PoseArray(head, cart_traj_msg)
00425         self.pose_traj_pub.publish(pose_array)
00426         self.force_monitor.update_activity()
00427 
00428 def main():
00429     rospy.init_node("face_adls_manager")
00430 
00431     #from pr2_traj_playback.arm_pose_move_controller import ArmPoseMoveBehavior, TrajectoryLoader
00432     #from pr2_traj_playback.arm_pose_move_controller import CTRL_NAME_LOW, PARAMS_FILE_LOW
00433     #r_apm = ArmPoseMoveBehavior('r', ctrl_name=CTRL_NAME_LOW,
00434     #                            param_file=PARAMS_FILE_LOW)
00435     #l_apm = ArmPoseMoveBehavior('l', ctrl_name=CTRL_NAME_LOW,
00436     #                            param_file=PARAMS_FILE_LOW)
00437     #traj_loader = TrajectoryLoader(r_apm, l_apm)
00438     #if False:
00439     #    traj_loader.move_to_setup_from_file("$(find hrl_face_adls)/data/l_arm_shaving_setup_r.pkl",
00440     #                                        velocity=0.1, reverse=False, blocking=True)
00441     #    traj_loader.exec_traj_from_file("$(find hrl_face_adls)/data/l_arm_shaving_setup_r.pkl",
00442     #                                    rate_mult=0.8, reverse=False, blocking=True)
00443     #if False:
00444     #    traj_loader.move_to_setup_from_file("$(find hrl_face_adls)/data/l_arm_shaving_setup_r.pkl",
00445     #                                        velocity=0.3, reverse=True, blocking=True)
00446 
00447     fam = FaceADLsManager()
00448     #fam.enable_controller()
00449     rospy.spin()
00450 
00451 if __name__ == "__main__":
00452     main()
00453 
00454     #def global_input_cb_old(self, msg):
00455     #    if self.is_forced_retreat:
00456     #        return
00457     #    if self.stop_move():
00458     #        rospy.loginfo("[face_adls_manager] Preempting other movement for global move.")
00459     #        #self.publish_feedback(Messages.GLOBAL_PREEMPT_OTHER)
00460     #    self.force_monitor.start_activity()
00461     #    goal_pose = self.global_poses[msg.data]
00462     #    goal_pose_name = msg.data
00463     #    self.publish_feedback(Messages.GLOBAL_START % goal_pose_name)
00464     #    try:
00465     #        if not self.ell_ctrl.execute_ell_move(((0, 0, RETREAT_HEIGHT), np.mat(np.eye(3))), 
00466     #                                              ((0, 0, 1), 1), 
00467     #                                              self.gripper_rot,
00468     #                                              APPROACH_VELOCITY,
00469     #                                              blocking=True):
00470     #            raise Exception
00471     #        if not self.ell_ctrl.execute_ell_move(((goal_pose[0][0], goal_pose[0][1], RETREAT_HEIGHT), (0, 0, 0, 1)), 
00472     #                                              ((1, 1, 1), 0), 
00473     #                                              self.gripper_rot,
00474     #                                              GLOBAL_VELOCITY,
00475     #                                              blocking=True):
00476     #            raise Exception
00477     #        final_goal = [goal_pose[0][0], goal_pose[0][1], goal_pose[0][2] - HEIGHT_CLOSER_ADJUST]
00478     #        if not self.ell_ctrl.execute_ell_move((final_goal, (0, 0, 0, 1)),
00479     #                                              ((1, 1, 1), 0), 
00480     #                                              self.gripper_rot,
00481     #                                              GLOBAL_VELOCITY,
00482     #                                              blocking=True):
00483     #            raise Exception
00484     #    except:
00485     #        self.publish_feedback(Messages.GLOBAL_PREEMPT % goal_pose_name)
00486     #        self.force_monitor.stop_activity()
00487     #        return
00488     #    self.publish_feedback(Messages.GLOBAL_SUCCESS % goal_pose_name)
00489     #    self.force_monitor.stop_activity()
00490 
00491 #    def local_input_cb(self, msg):
00492 #        if self.is_forced_retreat:
00493 #            return
00494 #        self.stop_moving()
00495 #        self.force_monitor.start_activity()
00496 #        button_press = msg.data 
00497 #
00498 #        curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
00499 #        curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
00500 #
00501 #        if button_press in ell_trans_params:
00502 #            self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
00503 #            change_trans_ep = ell_trans_params[button_press]
00504 #            goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
00505 #                            curr_ell_pos[1]+change_trans_ep[1],
00506 #                            curr_ell_pos[2]+change_trans_ep[2]]
00507 #            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00508 #                                                              goal_ell_pos, curr_ell_quat,
00509 #                                                              velocity=ELL_LOCAL_VEL, min_jerk=True)
00510 ##            success = self.ell_ctrl.execute_ell_move((change_trans_ep, (0, 0, 0, 1)),
00511 ##                                                     ((0, 0, 0), 0), 
00512 ##                                                    self.gripper_rot,
00513 ##                                                    ELL_LOCAL_VEL,
00514 ##                                                    blocking=True)
00515 #        elif button_press in ell_rot_params:
00516 #            self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
00517 #            change_rot_ep = ell_rot_params[button_press]
00518 #            rot_quat = trans.quaternion_from_euler(*change_rot_ep)
00519 #            goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
00520 #            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00521 #                                                              curr_ell_pos, goal_ell_quat,
00522 #                                                              velocity=ELL_ROT_VEL, min_jerk=True)
00523 #            #success = self.ell_ctrl.execute_ell_move(((0, 0, 0), rot_quat),
00524 #            #                                         ((0, 0, 0), 0), 
00525 #            #                                         self.gripper_rot,
00526 #            #                                         ELL_ROT_VEL,
00527 #            #                                         blocking=True)
00528 #        elif button_press == "reset_rotation":
00529 #            self.publish_feedback(Messages.ROT_RESET_START)
00530 #            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
00531 #                                                              curr_ell_pos,(0.,0.,0.,1.),
00532 #                                                              velocity=ELL_ROT_VEL, min_jerk=True)
00533 #            #success = self.ell_ctrl.execute_ell_move(((0, 0, 0), np.mat(np.eye(3))),
00534 #            #                                         ((0, 0, 0), 1), 
00535 #            #                                         self.gripper_rot,
00536 #            #                                         ELL_ROT_VEL,
00537 #            #                                         blocking=True)
00538 #        else:
00539 #            rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
00540 #
00541 #        cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
00542 #        cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
00543 #        head = Header()
00544 #        head.frame_id = '/ellipse_frame'
00545 #        head.stamp = rospy.Time.now()
00546 #        pose_array = PoseArray(head, cart_traj_msg)
00547 #        self.pose_traj_pub.publish(pose_array)
00548 #        #if success:
00549 #        #    self.publish_feedback(Messages.LOCAL_SUCCESS % button_names_dict[button_press])
00550 #        #else:
00551 #        #    self.publish_feedback(Messages.LOCAL_PREEMPT % button_names_dict[button_press])
00552 #        #self.force_monitor.stop_activity()