position_feedback_push_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 #  Copyright (c) 2011, Georgia Institute of Technology
00005 #  All rights reserved.
00006 #
00007 #  Redistribution and use in source and binary forms, with or without
00008 #  modification, are permitted provided that the following conditions
00009 #  are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #     notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #     copyright notice, this list of conditions and the following
00015 #     disclaimer in the documentation and/or other materials provided
00016 #     with the distribution.
00017 #  * Neither the name of the Georgia Institute of Technology nor the names of
00018 #     its contributors may be used to endorse or promote products derived
00019 #     from this software without specific prior written permission.
00020 #
00021 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 #  'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 #  POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib; roslib.load_manifest('tabletop_pushing')
00035 import rospy
00036 import actionlib
00037 import hrl_pr2_lib.pr2 as pr2
00038 import hrl_pr2_lib.pressure_listener as pl
00039 import hrl_lib.tf_utils as tfu
00040 from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher
00041 from geometry_msgs.msg import PoseStamped, TwistStamped
00042 from std_msgs.msg import Float64MultiArray
00043 from pr2_controllers_msgs.msg import *
00044 from pr2_manipulation_controllers.msg import *
00045 import tf
00046 import numpy as np
00047 from tabletop_pushing.srv import *
00048 from tabletop_pushing.msg import *
00049 from math import sin, cos, pi, fabs, sqrt
00050 import sys
00051 
00052 # Setup joints stolen from Kelsey's code.
00053 LEFT_ARM_SETUP_JOINTS = np.matrix([[1.32734204881265387,
00054                                     -0.34601608409943324,
00055                                     1.4620635485239604,
00056                                     -1.2729772622637399,
00057                                     7.5123303230158518,
00058                                     -1.5570651396529178,
00059                                     -5.5929916630672727]]).T
00060 RIGHT_ARM_SETUP_JOINTS = np.matrix([[-1.32734204881265387,
00061                                       -0.34601608409943324,
00062                                       -1.4620635485239604,
00063                                       -1.2729772622637399,
00064                                       -7.5123303230158518,
00065                                       -1.5570651396529178,
00066                                       -7.163787989862169]]).T
00067 LEFT_ARM_READY_JOINTS = np.matrix([[0.42427649, 0.0656137,
00068                                     1.43411927, -2.11931035,
00069                                     -15.78839978, -1.64163257,
00070                                     -17.2947453]]).T
00071 RIGHT_ARM_READY_JOINTS = np.matrix([[-0.42427649, 0.0656137,
00072                                      -1.43411927, -2.11931035,
00073                                      15.78839978, -1.64163257,
00074                                      8.64421842e+01]]).T
00075 LEFT_ARM_PULL_READY_JOINTS = np.matrix([[0.42427649,
00076                                          -0.34601608409943324,
00077                                          1.43411927,
00078                                          -2.11931035,
00079                                          82.9984037,
00080                                          -1.64163257,
00081                                          -36.16]]).T
00082 RIGHT_ARM_PULL_READY_JOINTS = np.matrix([[-0.42427649,
00083                                          -0.34601608409943324,
00084                                          -1.43411927,
00085                                          -2.11931035,
00086                                          -82.9984037,
00087                                          -1.64163257,
00088                                          54.8]]).T
00089 LEFT_ARM_HIGH_PUSH_READY_JOINTS = np.matrix([[0.42427649,
00090                                               -0.34601608409943324,
00091                                               1.43411927,
00092                                               -2.11931035,
00093                                               -15.78839978,
00094                                               -1.64163257,
00095                                               -17.2947453]]).T
00096 RIGHT_ARM_HIGH_PUSH_READY_JOINTS = np.matrix([[-0.42427649,
00097                                                 -0.34601608409943324,
00098                                                 -1.43411927,
00099                                                 -2.11931035,
00100                                                 15.78839978,
00101                                                 -1.64163257,
00102                                                 8.64421842e+01]]).T
00103 LEFT_ARM_HIGH_SWEEP_READY_JOINTS = LEFT_ARM_HIGH_PUSH_READY_JOINTS
00104 RIGHT_ARM_HIGH_SWEEP_READY_JOINTS = RIGHT_ARM_HIGH_PUSH_READY_JOINTS
00105 
00106 _POSTURES = {
00107     'off': [],
00108     'mantis': [0, 1, 0,  -1, 3.14, -1, 3.14],
00109     'elbowupr': [-0.79,0,-1.6,  9999, 9999, 9999, 9999],
00110     'elbowupl': [0.79,0,1.6 , 9999, 9999, 9999, 9999],
00111     'old_elbowupr': [-0.79,0,-1.6, -0.79,3.14, -0.79,5.49],
00112     'old_elbowupl': [0.79,0,1.6, -0.79,3.14, -0.79,5.49],
00113     'elbowdownr': [-0.028262077316910873, 1.2946342642324222, -0.25785640577652386, -1.5498884526859626, -31.278913849571776, -1.0527644894829107, -1.8127318367654268],
00114     'elbowdownl': [-0.0088195719039858515, 1.2834828245284853, 0.20338442004843196, -1.5565279256852611, -0.096340012666916802, -1.0235018652439782, 1.7990893054129216]
00115 }
00116 
00117 class PositionFeedbackPushNode:
00118 
00119     def __init__(self):
00120         rospy.init_node('position_feedback_push_node', log_level=rospy.DEBUG)
00121         # Setup parameters
00122         self.torso_z_offset = rospy.get_param('~torso_z_offset', 0.30)
00123         self.look_pt_x = rospy.get_param('~look_point_x', 0.7)
00124         self.head_pose_cam_frame = rospy.get_param('~head_pose_cam_frame',
00125                                                    'head_mount_kinect_rgb_link')
00126         self.default_torso_height = rospy.get_param('~default_torso_height',
00127                                                     0.28)
00128         self.gripper_raise_dist = rospy.get_param('~gripper_raise_dist',
00129                                                   0.05)
00130         self.high_arm_init_z = rospy.get_param('~high_arm_start_z', 0.15)
00131         self.post_controller_switch_sleep = rospy.get_param(
00132             '~arm_switch_sleep_time', 0.5)
00133         self.move_cart_check_hz = rospy.get_param('~move_cart_check_hz', 100)
00134         self.arm_done_moving_count_thresh = rospy.get_param(
00135             '~not_moving_count_thresh', 30)
00136         self.arm_done_moving_epc_count_thresh = rospy.get_param(
00137             '~not_moving_epc_count_thresh', 60)
00138         self.post_move_count_thresh = rospy.get_param('~post_move_count_thresh',
00139                                                       10)
00140         self.pre_push_count_thresh = rospy.get_param('~pre_push_count_thresh',
00141                                                       50)
00142         self.still_moving_velocity = rospy.get_param('~moving_vel_thresh', 0.01)
00143         self.still_moving_angular_velocity = rospy.get_param('~angular_vel_thresh',
00144                                                              0.005)
00145         self.pressure_safety_limit = rospy.get_param('~pressure_limit',
00146                                                      2000)
00147         self.use_jinv = rospy.get_param('~use_jinv', True)
00148         self.use_cur_joint_posture = rospy.get_param('~use_joint_posture', True)
00149         # Setup cartesian controller parameters
00150         if self.use_jinv:
00151             self.base_cart_controller_name = '_cart_jinv_push'
00152             self.controller_state_msg = JinvTeleopControllerState
00153         else:
00154             self.base_cart_controller_name = '_cart_transpose_push'
00155             self.controller_state_msg = JTTaskControllerState
00156         self.base_vel_controller_name = '_cart_jinv_push'
00157         self.vel_controller_state_msg = JinvTeleopControllerState
00158 
00159         # Set joint gains
00160         rospy.loginfo('Craeting cs object')
00161         self.arm_mode = None
00162         self.cs = ControllerSwitcher()
00163         self.init_joint_controllers()
00164         self.init_cart_controllers()
00165         self.init_vel_controllers()
00166 
00167         # Setup arms
00168         self.tf_listener = tf.TransformListener()
00169         rospy.loginfo('Creating pr2 object')
00170         self.robot = pr2.PR2(self.tf_listener, arms=True, base=False, 
00171                              use_kinematics=False)#, use_projector=False)
00172 
00173         self.l_arm_cart_pub = rospy.Publisher(
00174             '/l'+self.base_cart_controller_name+'/command_pose', PoseStamped)
00175         self.r_arm_cart_pub = rospy.Publisher(
00176             '/r'+self.base_cart_controller_name+'/command_pose', PoseStamped)
00177         self.l_arm_cart_posture_pub = rospy.Publisher(
00178             '/l'+self.base_cart_controller_name+'/command_posture',
00179             Float64MultiArray)
00180         self.r_arm_cart_posture_pub = rospy.Publisher(
00181             '/r'+self.base_cart_controller_name+'/command_posture',
00182             Float64MultiArray)
00183         self.l_arm_cart_vel_pub = rospy.Publisher(
00184             '/l'+self.base_vel_controller_name+'/command_twist', TwistStamped)
00185         self.r_arm_cart_vel_pub = rospy.Publisher(
00186             '/r'+self.base_vel_controller_name+'/command_twist', TwistStamped)
00187         self.l_arm_vel_posture_pub = rospy.Publisher(
00188             '/l'+self.base_vel_controller_name+'/command_posture',
00189             Float64MultiArray)
00190         self.r_arm_vel_posture_pub = rospy.Publisher(
00191             '/r'+self.base_vel_controller_name+'/command_posture',
00192             Float64MultiArray)
00193 
00194         rospy.Subscriber('/l'+self.base_cart_controller_name+'/state',
00195                          self.controller_state_msg,
00196                          self.l_arm_cart_state_callback)
00197         rospy.Subscriber('/r'+self.base_cart_controller_name+'/state',
00198                          self.controller_state_msg,
00199                          self.r_arm_cart_state_callback)
00200 
00201         # TODO: setup velocity controller callback methods
00202         # rospy.Subscriber('/l'+self.base_vel_controller_name+'/state',
00203         #                  self.vel_controller_state_msg,
00204         #                  self.l_arm_vel_state_callback)
00205         # rospy.Subscriber('/r'+self.base_vel_controller_name+'/state',
00206         #                  self.vel_controller_state_msg,
00207         #                  self.r_arm_vel_state_callback)
00208 
00209         self.l_pressure_listener = pl.PressureListener(
00210             '/pressure/l_gripper_motor', self.pressure_safety_limit)
00211         self.r_pressure_listener = pl.PressureListener(
00212             '/pressure/r_gripper_motor', self.pressure_safety_limit)
00213 
00214 
00215         # State Info
00216         self.l_arm_pose = None
00217         self.l_arm_x_err = None
00218         self.l_arm_x_d = None
00219         self.l_arm_F = None
00220 
00221         self.r_arm_pose = None
00222         self.r_arm_x_err = None
00223         self.r_arm_x_d = None
00224         self.r_arm_F = None
00225 
00226         # Open callback services
00227         self.gripper_feedback_push_srv = rospy.Service(
00228             'gripper_feedback_push', GripperPush, self.gripper_feedback_push)
00229         self.gripper_pre_push_srv = rospy.Service('gripper_pre_push',
00230                                                   GripperPush,
00231                                                   self.gripper_pre_push)
00232         self.gripper_push_srv = rospy.Service('gripper_push', GripperPush,
00233                                               self.gripper_push)
00234         self.gripper_post_push_srv = rospy.Service('gripper_post_push',
00235                                                    GripperPush,
00236                                                    self.gripper_post_push)
00237 
00238         self.gripper_pre_sweep_srv = rospy.Service('gripper_pre_sweep',
00239                                                    GripperPush,
00240                                                    self.gripper_pre_sweep)
00241         self.gripper_sweep_srv = rospy.Service('gripper_sweep',
00242                                                GripperPush,
00243                                                self.gripper_sweep)
00244         self.gripper_post_sweep_srv = rospy.Service('gripper_post_sweep',
00245                                                     GripperPush,
00246                                                     self.gripper_post_sweep)
00247 
00248         self.overhead_pre_push_srv = rospy.Service('overhead_pre_push',
00249                                                    GripperPush,
00250                                                    self.overhead_pre_push)
00251         self.overhead_push_srv = rospy.Service('overhead_push',
00252                                                GripperPush,
00253                                                self.overhead_push)
00254         self.overhead_post_push_srv = rospy.Service('overhead_post_push',
00255                                                    GripperPush,
00256                                                    self.overhead_post_push)
00257 
00258         self.raise_and_look_serice = rospy.Service('raise_and_look',
00259                                                    RaiseAndLook,
00260                                                    self.raise_and_look)
00261     #
00262     # Initialization functions
00263     #
00264 
00265     #
00266     # Arm pose initialization functions
00267     #
00268     def init_arm_pose(self, force_ready=False, which_arm='l'):
00269         '''
00270         Move the arm to the initial pose to be out of the way for viewing the
00271         tabletop
00272         '''
00273         if which_arm == 'l':
00274             robot_gripper = self.robot.left_gripper
00275             ready_joints = LEFT_ARM_READY_JOINTS
00276             setup_joints = LEFT_ARM_SETUP_JOINTS
00277         else:
00278             robot_gripper = self.robot.right_gripper
00279             ready_joints = RIGHT_ARM_READY_JOINTS
00280             setup_joints = RIGHT_ARM_SETUP_JOINTS
00281 
00282         rospy.loginfo('Moving %s_arm to setup pose' % which_arm)
00283         self.set_arm_joint_pose(setup_joints, which_arm)
00284         rospy.loginfo('Moved %s_arm to setup pose' % which_arm)
00285 
00286         #rospy.loginfo('Closing %s_gripper' % which_arm)
00287         #res = robot_gripper.close(block=True)
00288         #rospy.loginfo('Closed %s_gripper' % which_arm)
00289 
00290 
00291     def reset_arm_pose(self, force_ready=False, which_arm='l',
00292                        high_arm_joints=False):
00293         '''
00294         Move the arm to the initial pose to be out of the way for viewing the
00295         tabletop
00296         '''
00297         if which_arm == 'l':
00298             robot_gripper = self.robot.left_gripper
00299             if high_arm_joints:
00300                 ready_joints = LEFT_ARM_HIGH_PUSH_READY_JOINTS
00301             else:
00302                 ready_joints = LEFT_ARM_READY_JOINTS
00303             setup_joints = LEFT_ARM_SETUP_JOINTS
00304         else:
00305             robot_gripper = self.robot.right_gripper
00306             if high_arm_joints:
00307                 ready_joints = RIGHT_ARM_HIGH_PUSH_READY_JOINTS
00308             else:
00309                 ready_joints = RIGHT_ARM_READY_JOINTS
00310             setup_joints = RIGHT_ARM_SETUP_JOINTS
00311         ready_diff = np.linalg.norm(pr2.diff_arm_pose(self.get_arm_joint_pose(which_arm),
00312                                                       ready_joints))
00313 
00314         # Choose to move to ready first, if it is closer, then move to init
00315         if force_ready or ready_diff > READY_POSE_MOVE_THRESH:
00316             rospy.logdebug('Moving %s_arm to ready pose' % which_arm)
00317             self.set_arm_joint_pose(ready_joints, which_arm, nsecs=1.5)
00318             rospy.logdebug('Moved %s_arm to ready pose' % which_arm)
00319         else:
00320             rospy.logdebug('Arm in ready pose')
00321 
00322         rospy.logdebug('Moving %s_arm to setup pose' % which_arm)
00323         self.set_arm_joint_pose(setup_joints, which_arm, nsecs=1.5)
00324         rospy.logdebug('Moved %s_arm to setup pose' % which_arm)
00325 
00326     def init_head_pose(self, camera_frame):
00327         look_pt = np.asmatrix([self.look_pt_x, 0.0, -self.torso_z_offset])
00328         rospy.loginfo('Point head at ' + str(look_pt))
00329         head_res = self.robot.head.look_at(look_pt,
00330                                            'torso_lift_link',
00331                                            camera_frame)
00332         if head_res:
00333             rospy.loginfo('Succeeded in pointing head')
00334             return True
00335         else:
00336             rospy.logwarn('Failed to point head')
00337             return False
00338 
00339     def init_spine_pose(self):
00340         rospy.loginfo('Setting spine height to '+str(self.default_torso_height))
00341         self.robot.torso.set_pose(self.default_torso_height)
00342         new_torso_position = np.asarray(self.robot.torso.pose()).ravel()[0]
00343         rospy.loginfo('New spine height is ' + str(new_torso_position))
00344 
00345     def init_arms(self):
00346         self.init_arm_pose(True, which_arm='r')
00347         self.init_arm_pose(True, which_arm='l')
00348         rospy.loginfo('Done initializing arms')
00349 
00350     #
00351     # Behavior functions
00352     #
00353     def gripper_feedback_push(self, request):
00354         response = GripperPushResponse()
00355         start_point = request.start_point.point
00356         wrist_yaw = request.wrist_yaw
00357         push_dist = request.desired_push_dist
00358 
00359         if request.left_arm:
00360             ready_joints = LEFT_ARM_READY_JOINTS
00361             which_arm = 'l'
00362         else:
00363             ready_joints = RIGHT_ARM_READY_JOINTS
00364             which_arm = 'r'
00365 
00366         self.active_arm = which_arm
00367         rospy.loginfo('Creating ac')
00368         ac = actionlib.SimpleActionClient('push_tracker',
00369                                           VisFeedbackPushTrackingAction)
00370         rospy.loginfo('waiting for server')
00371         if not ac.wait_for_server(rospy.Duration(5.0)):
00372             rospy.loginfo('Failed to connect to push tracker server')
00373             return response
00374         ac.cancel_all_goals()
00375         self.feedback_count = 0
00376 
00377         # Start pushing forward
00378         # self.vel_push_forward(which_arm)
00379         done_cb = None
00380         active_cb = None
00381         feedback_cb = self.tracker_feedback_gripper_push
00382         goal = VisFeedbackPushTrackingGoal()
00383         goal.which_arm = which_arm
00384         goal.header.frame_id = request.start_point.header.frame_id
00385         goal.desired_pose = request.goal_pose
00386         self.desired_pose = request.goal_pose
00387         rospy.loginfo('Sending goal of: ' + str(goal.desired_pose))
00388         ac.send_goal(goal, done_cb, active_cb, feedback_cb)
00389         # Block until done
00390         rospy.loginfo('Waiting for result')
00391         ac.wait_for_result(rospy.Duration(0))
00392         rospy.loginfo('Result received')
00393         self.stop_moving_vel(which_arm)
00394 
00395         # TODO: send back info
00396         return response
00397 
00398     def tracker_feedback_gripper_push(self, feedback):
00399         which_arm = self.active_arm
00400         update_twist = TwistStamped()
00401         update_twist.header.frame_id = 'torso_lift_link'
00402         update_twist.header.stamp = rospy.Time(0)
00403         update_twist.twist.linear.z = 0.0
00404         update_twist.twist.angular.x = 0.0
00405         update_twist.twist.angular.y = 0.0
00406         update_twist.twist.angular.z = 0.0
00407 
00408         # Push centroid towards the desired goal
00409         goal_x_dot = self.desired_pose.x - feedback.x.x
00410         goal_y_dot = self.desired_pose.y - feedback.x.y
00411 
00412         # Add in direction to corect for spinning
00413         # TODO: Correct component based on transform in object pose
00414         spin_x_dot = sin(feedback.x.theta)*feedback.x_dot.theta
00415         spin_y_dot = cos(feedback.x.theta)*feedback.x_dot.theta
00416 
00417         k_g = 0.1
00418         k_s = 0.03
00419 
00420         update_twist.twist.linear.x = k_g*goal_x_dot + k_s*spin_x_dot
00421         update_twist.twist.linear.y = k_g*goal_y_dot + k_s*spin_y_dot
00422         if self.feedback_count % 15 == 0:
00423             rospy.loginfo('Desired pose: ' + str(self.desired_pose))
00424             rospy.loginfo('Current pose: ' + str(feedback.x))
00425             rospy.loginfo('q_goal_dot: (' + str(k_g*goal_x_dot) + ', ' + str(k_g*goal_y_dot) + ')')
00426             rospy.loginfo('q_spin_dot: (' + str(spin_x_dot) + ', ' + str(spin_y_dot) + ')')
00427             rospy.loginfo('q_dot: (' + str(update_twist.twist.linear.x) + ',' + str(update_twist.twist.linear.y) + ')\n')
00428         update_twist.twist.linear.x = k_g*goal_x_dot
00429         update_twist.twist.linear.y = k_g*goal_y_dot
00430         self.update_vel(update_twist, which_arm)
00431         self.feedback_count += 1
00432 
00433     def gripper_push(self, request):
00434         response = GripperPushResponse()
00435         start_point = request.start_point.point
00436         wrist_yaw = request.wrist_yaw
00437         push_dist = request.desired_push_dist
00438 
00439         if request.left_arm:
00440             ready_joints = LEFT_ARM_READY_JOINTS
00441             which_arm = 'l'
00442         else:
00443             ready_joints = RIGHT_ARM_READY_JOINTS
00444             which_arm = 'r'
00445 
00446         rospy.loginfo('Pushing forward ' + str(push_dist) + 'm')
00447         # pose_err, err_dist = self.move_relative_gripper(
00448         #     np.matrix([push_dist, 0.0, 0.0]).T, which_arm)
00449         # pose_err, err_dist = self.move_relative_torso(
00450         #     np.matrix([cos(wrist_yaw)*push_dist,
00451         #                sin(wrist_yaw)*push_dist, 0.0]).T, which_arm)
00452         pose_err, err_dist = self.move_relative_torso_epc(wrist_yaw, push_dist,
00453                                                           which_arm)
00454         rospy.loginfo('Done pushing forward')
00455 
00456         response.dist_pushed = push_dist - err_dist
00457         return response
00458 
00459     def gripper_pre_push(self, request):
00460         response = GripperPushResponse()
00461         start_point = request.start_point.point
00462         wrist_yaw = request.wrist_yaw
00463         push_dist = request.desired_push_dist
00464         if request.left_arm:
00465             ready_joints = LEFT_ARM_READY_JOINTS
00466             if request.high_arm_init:
00467                 ready_joints = LEFT_ARM_HIGH_PUSH_READY_JOINTS
00468             which_arm = 'l'
00469             robot_gripper = self.robot.left_gripper
00470         else:
00471             ready_joints = RIGHT_ARM_READY_JOINTS
00472             if request.high_arm_init:
00473                 ready_joints = RIGHT_ARM_HIGH_PUSH_READY_JOINTS
00474             which_arm = 'r'
00475             robot_gripper = self.robot.right_gripper
00476 
00477         if request.arm_init:
00478             self.set_arm_joint_pose(ready_joints, which_arm, nsecs=1.5)
00479             rospy.logdebug('Moving %s_arm to ready pose' % which_arm)
00480 
00481         start_pose = PoseStamped()
00482         start_pose.header = request.start_point.header
00483         start_pose.pose.position.x = start_point.x
00484         start_pose.pose.position.y = start_point.y
00485         start_pose.pose.position.z = start_point.z
00486         q = tf.transformations.quaternion_from_euler(0.0, -0.1, wrist_yaw)
00487         start_pose.pose.orientation.x = q[0]
00488         start_pose.pose.orientation.y = q[1]
00489         start_pose.pose.orientation.z = q[2]
00490         start_pose.pose.orientation.w = q[3]
00491 
00492         if request.open_gripper:
00493             # TODO: open gripper
00494             res = robot_gripper.open(block=True, position=0.05)
00495 
00496         if request.high_arm_init:
00497             start_pose.pose.position.z = self.high_arm_init_z
00498             self.move_to_cart_pose(start_pose, which_arm)
00499             rospy.logdebug('Done moving to overhead start point')
00500             # Change z to lower arm to table
00501             start_pose.pose.position.z = start_point.z
00502             # self.move_down_until_contact(which_arm)
00503 
00504         # Move to start pose
00505         self.move_to_cart_pose(start_pose, which_arm,
00506                                self.pre_push_count_thresh)
00507         rospy.loginfo('Done moving to start point')
00508         return response
00509 
00510     def gripper_post_push(self, request):
00511         response = GripperPushResponse()
00512         start_point = request.start_point.point
00513         wrist_yaw = request.wrist_yaw
00514         push_dist = request.desired_push_dist
00515 
00516         if request.left_arm:
00517             ready_joints = LEFT_ARM_READY_JOINTS
00518             which_arm = 'l'
00519             robot_gripper = self.robot.left_gripper
00520         else:
00521             ready_joints = RIGHT_ARM_READY_JOINTS
00522             which_arm = 'r'
00523             robot_gripper = self.robot.right_gripper
00524 
00525         # Retract in a straight line
00526         rospy.logdebug('Moving gripper up')
00527         pose_err, err_dist = self.move_relative_gripper(
00528             np.matrix([0.0, 0.0, self.gripper_raise_dist]).T, which_arm,
00529             move_cart_count_thresh=self.post_move_count_thresh)
00530         rospy.logdebug('Moving gripper backwards')
00531         pose_err, err_dist = self.move_relative_gripper(
00532             np.matrix([-push_dist, 0.0, 0.0]).T, which_arm,
00533             move_cart_count_thresh=self.post_move_count_thresh)
00534         rospy.loginfo('Done moving backwards')
00535         start_pose = PoseStamped()
00536         start_pose.header = request.start_point.header
00537         start_pose.pose.position.x = start_point.x
00538         start_pose.pose.position.y = start_point.y
00539         start_pose.pose.position.z = start_point.z
00540         q = tf.transformations.quaternion_from_euler(0.0, 0.0, wrist_yaw)
00541         start_pose.pose.orientation.x = q[0]
00542         start_pose.pose.orientation.y = q[1]
00543         start_pose.pose.orientation.z = q[2]
00544         start_pose.pose.orientation.w = q[3]
00545 
00546         if request.open_gripper:
00547             # TODO: Close gripper
00548             res = robot_gripper.close(block=True)
00549 
00550         if request.high_arm_init:
00551             start_pose.pose.position.z = self.high_arm_init_z
00552             rospy.logdebug('Moving up to initial point')
00553             self.move_to_cart_pose(start_pose, which_arm,
00554                                    self.post_move_count_thresh)
00555             rospy.loginfo('Done moving up to initial point')
00556 
00557         if request.arm_reset:
00558             self.reset_arm_pose(True, which_arm, request.high_arm_init)
00559         return response
00560 
00561     def gripper_sweep(self, request):
00562         response = GripperPushResponse()
00563         start_point = request.start_point.point
00564         wrist_yaw = request.wrist_yaw
00565         push_dist = request.desired_push_dist
00566 
00567         if request.left_arm:
00568             ready_joints = LEFT_ARM_READY_JOINTS
00569             which_arm = 'l'
00570             wrist_roll = -pi
00571         else:
00572             ready_joints = RIGHT_ARM_READY_JOINTS
00573             which_arm = 'r'
00574             wrist_roll = 0.0
00575 
00576         # NOTE: because of the wrist roll orientation, +Z at the gripper
00577         # equates to negative Y in the torso_lift_link at 0.0 yaw
00578         # So we flip the push_dist to make things look like one would expect
00579         rospy.loginfo('Sweeping gripper in ' + str(push_dist) + 'm')
00580         # pose_err, err_dist = self.move_relative_gripper(
00581         #     np.matrix([0.0, 0.0, -push_dist]).T, which_arm)
00582         if wrist_yaw > -pi*0.5:
00583             push_angle = wrist_yaw + pi*0.5
00584         else:
00585             push_angle = wrist_yaw - pi*0.5
00586         # pose_err, err_dist = self.move_relative_torso(
00587         #     np.matrix([cos(push_angle)*push_dist,
00588         #                sin(push_angle)*push_dist, 0.0]).T, which_arm)
00589         pose_err, err_dist = self.move_relative_torso_epc(push_angle, push_dist,
00590                                                           which_arm)
00591 
00592         rospy.logdebug('Done sweeping in')
00593 
00594         # response.dist_pushed = push_dist - err_dist
00595         return response
00596 
00597     def gripper_pre_sweep(self, request):
00598         response = GripperPushResponse()
00599         start_point = request.start_point.point
00600         wrist_yaw = request.wrist_yaw
00601         push_dist = request.desired_push_dist
00602 
00603         if request.left_arm:
00604             ready_joints = LEFT_ARM_READY_JOINTS
00605             if request.high_arm_init:
00606                 ready_joints = LEFT_ARM_HIGH_SWEEP_READY_JOINTS
00607             which_arm = 'l'
00608             wrist_roll = -pi
00609         else:
00610             ready_joints = RIGHT_ARM_READY_JOINTS
00611             if request.high_arm_init:
00612                 ready_joints = RIGHT_ARM_HIGH_SWEEP_READY_JOINTS
00613             which_arm = 'r'
00614             wrist_roll = 0.0
00615 
00616         start_pose = PoseStamped()
00617         start_pose.header = request.start_point.header
00618         start_pose.pose.position.x = start_point.x
00619         start_pose.pose.position.y = start_point.y
00620         start_pose.pose.position.z = start_point.z
00621         q = tf.transformations.quaternion_from_euler(0.5*pi, 0.0, wrist_yaw)
00622         start_pose.pose.orientation.x = q[0]
00623         start_pose.pose.orientation.y = q[1]
00624         start_pose.pose.orientation.z = q[2]
00625         start_pose.pose.orientation.w = q[3]
00626 
00627         if request.arm_init:
00628             rospy.logdebug('Moving %s_arm to ready pose' % which_arm)
00629             self.set_arm_joint_pose(ready_joints, which_arm, nsecs=1.5)
00630             # Rotate wrist before moving to position
00631             rospy.logdebug('Rotating wrist for sweep')
00632             arm_pose = self.get_arm_joint_pose(which_arm)
00633             arm_pose[-1] =  wrist_roll
00634             self.set_arm_joint_pose(arm_pose, which_arm, nsecs=1.0)
00635         if request.high_arm_init:
00636             # Move to offset pose above the table
00637             start_pose.pose.position.z = self.high_arm_init_z
00638             self.move_to_cart_pose(start_pose, which_arm)
00639             rospy.logdebug('Done moving to overhead start point')
00640             # Lower arm to table
00641             start_pose.pose.position.z = start_point.z
00642             # self.move_down_until_contact(which_arm)
00643 
00644         self.move_to_cart_pose(start_pose, which_arm,
00645                                self.pre_push_count_thresh)
00646         rospy.loginfo('Done moving to start point')
00647 
00648         return response
00649 
00650     def gripper_post_sweep(self, request):
00651         response = GripperPushResponse()
00652         start_point = request.start_point.point
00653         wrist_yaw = request.wrist_yaw
00654         push_dist = request.desired_push_dist
00655 
00656         if request.left_arm:
00657             ready_joints = LEFT_ARM_READY_JOINTS
00658             which_arm = 'l'
00659         else:
00660             ready_joints = RIGHT_ARM_READY_JOINTS
00661             which_arm = 'r'
00662 
00663         rospy.logdebug('Moving gripper up')
00664         pose_err, err_dist = self.move_relative_gripper(
00665             np.matrix([0.0, self.gripper_raise_dist, 0.0]).T, which_arm,
00666             move_cart_count_thresh=self.post_move_count_thresh)
00667         rospy.logdebug('Sweeping gripper outward')
00668         pose_err, err_dist = self.move_relative_gripper(
00669             np.matrix([0.0, 0.0, push_dist]).T, which_arm,
00670             move_cart_count_thresh=self.post_move_count_thresh)
00671         rospy.loginfo('Done sweeping outward')
00672 
00673         if request.high_arm_init:
00674             rospy.logdebug('Moving up to end point')
00675             wrist_yaw = request.wrist_yaw
00676             end_pose = PoseStamped()
00677             end_pose.header = request.start_point.header
00678             end_pose.pose.position.x = start_point.x
00679             end_pose.pose.position.y = start_point.y
00680             end_pose.pose.position.z = self.high_arm_init_z
00681             q = tf.transformations.quaternion_from_euler(0.5*pi, 0.0, wrist_yaw)
00682             end_pose.pose.orientation.x = q[0]
00683             end_pose.pose.orientation.y = q[1]
00684             end_pose.pose.orientation.z = q[2]
00685             end_pose.pose.orientation.w = q[3]
00686             self.move_to_cart_pose(end_pose, which_arm,
00687                                    self.post_move_count_thresh)
00688             rospy.loginfo('Done moving up to end point')
00689 
00690         if request.arm_reset:
00691             self.reset_arm_pose(True, which_arm, request.high_arm_init)
00692         return response
00693 
00694     def overhead_push(self, request):
00695         response = GripperPushResponse()
00696         start_point = request.start_point.point
00697         wrist_yaw = request.wrist_yaw
00698         push_dist = request.desired_push_dist
00699 
00700         if request.left_arm:
00701             which_arm = 'l'
00702             wrist_pitch = 0.5*pi
00703         else:
00704             which_arm = 'r'
00705             wrist_pitch = -0.5*pi
00706 
00707         rospy.loginfo('Pushing forward ' + str(push_dist) + 'm')
00708         # pose_err, err_dist = self.move_relative_gripper(
00709         #     np.matrix([0.0, 0.0, push_dist]).T, which_arm)
00710         # pose_err, err_dist = self.move_relative_torso(
00711         #     np.matrix([cos(wrist_yaw)*push_dist,
00712         #                sin(wrist_yaw)*push_dist, 0.0]).T, which_arm)
00713         pose_err, err_dist = self.move_relative_torso_epc(wrist_yaw, push_dist,
00714                                                           which_arm)
00715 
00716         rospy.logdebug('Done pushing forward')
00717 
00718         # TODO: Add this back in
00719         response.dist_pushed = push_dist - err_dist
00720         return response
00721 
00722     def overhead_pre_push(self, request):
00723         response = GripperPushResponse()
00724         start_point = request.start_point.point
00725         wrist_yaw = request.wrist_yaw
00726         push_dist = request.desired_push_dist
00727 
00728         if request.left_arm:
00729             ready_joints = LEFT_ARM_READY_JOINTS
00730             if request.high_arm_init:
00731                 ready_joints = LEFT_ARM_PULL_READY_JOINTS
00732             which_arm = 'l'
00733             wrist_pitch = 0.5*pi
00734         else:
00735             ready_joints = RIGHT_ARM_READY_JOINTS
00736             if request.high_arm_init:
00737                 ready_joints = RIGHT_ARM_PULL_READY_JOINTS
00738             which_arm = 'r'
00739             wrist_pitch = -0.5*pi
00740 
00741         start_pose = PoseStamped()
00742         start_pose.header = request.start_point.header
00743         start_pose.pose.position.x = start_point.x
00744         start_pose.pose.position.y = start_point.y
00745         start_pose.pose.position.z = start_point.z
00746         q = tf.transformations.quaternion_from_euler(0.0, fabs(wrist_pitch),
00747                                                      wrist_yaw)
00748         start_pose.pose.orientation.x = q[0]
00749         start_pose.pose.orientation.y = q[1]
00750         start_pose.pose.orientation.z = q[2]
00751         start_pose.pose.orientation.w = q[3]
00752 
00753         if request.arm_init:
00754             rospy.logdebug('Moving %s_arm to ready pose' % which_arm)
00755             self.set_arm_joint_pose(ready_joints, which_arm, nsecs=1.5)
00756 
00757             if not request.high_arm_init:
00758                 # Rotate wrist before moving to position
00759                 rospy.logdebug('Rotating elbow for overhead push')
00760                 arm_pose = self.get_arm_joint_pose(which_arm)
00761                 arm_pose[-3] =  wrist_pitch
00762                 self.set_arm_joint_pose(arm_pose, which_arm, nsecs=1.0)
00763 
00764         if request.high_arm_init:
00765             # Move to offset pose above the table
00766             start_pose.pose.position.z = self.high_arm_init_z
00767             self.move_to_cart_pose(start_pose, which_arm)
00768             rospy.logdebug('Done moving to overhead start point')
00769             # Lower arm to table
00770             start_pose.pose.position.z = start_point.z
00771             # self.move_down_until_contact(which_arm)
00772 
00773         # Move to offset pose
00774         self.move_to_cart_pose(start_pose, which_arm,
00775                                self.pre_push_count_thresh)
00776         rospy.loginfo('Done moving to start point')
00777 
00778         return response
00779 
00780     def overhead_post_push(self, request):
00781         response = GripperPushResponse()
00782         start_point = request.start_point.point
00783         wrist_yaw = request.wrist_yaw
00784         push_dist = request.desired_push_dist
00785 
00786         if request.left_arm:
00787             ready_joints = LEFT_ARM_READY_JOINTS
00788             if request.high_arm_init:
00789                 ready_joints = LEFT_ARM_PULL_READY_JOINTS
00790             which_arm = 'l'
00791             wrist_pitch = 0.5*pi
00792         else:
00793             ready_joints = RIGHT_ARM_READY_JOINTS
00794             if request.high_arm_init:
00795                 ready_joints = RIGHT_ARM_PULL_READY_JOINTS
00796             which_arm = 'r'
00797             wrist_pitch = -0.5*pi
00798 
00799         rospy.logdebug('Moving gripper up')
00800         pose_err, err_dist = self.move_relative_gripper(
00801             np.matrix([-self.gripper_raise_dist, 0.0, 0.0]).T, which_arm,
00802             move_cart_count_thresh=self.post_move_count_thresh)
00803         rospy.logdebug('Done moving up')
00804         rospy.logdebug('Pushing reverse')
00805         pose_err, err_dist = self.move_relative_gripper(
00806             np.matrix([0.0, 0.0, -push_dist]).T, which_arm,
00807             move_cart_count_thresh=self.post_move_count_thresh)
00808         rospy.loginfo('Done pushing reverse')
00809 
00810         if request.high_arm_init:
00811             rospy.logdebug('Moving up to end point')
00812             wrist_yaw = request.wrist_yaw
00813             end_pose = PoseStamped()
00814             end_pose.header = request.start_point.header
00815             end_pose.pose.position.x = start_point.x
00816             end_pose.pose.position.y = start_point.y
00817             end_pose.pose.position.z = self.high_arm_init_z
00818             q = tf.transformations.quaternion_from_euler(0.0, 0.5*pi, wrist_yaw)
00819             end_pose.pose.orientation.x = q[0]
00820             end_pose.pose.orientation.y = q[1]
00821             end_pose.pose.orientation.z = q[2]
00822             end_pose.pose.orientation.w = q[3]
00823             self.move_to_cart_pose(end_pose, which_arm,
00824                                    self.post_move_count_thresh)
00825             rospy.loginfo('Done moving up to end point')
00826 
00827         if request.arm_reset:
00828             self.reset_arm_pose(True, which_arm, request.high_arm_init)
00829         return response
00830 
00831     def raise_and_look(self, request):
00832         '''
00833         Service callback to raise the spine to a specific height relative to the
00834         table height and tilt the head so that the Kinect views the table
00835         '''
00836         if request.init_arms:
00837             self.init_arms()
00838 
00839         if request.point_head_only:
00840             response = RaiseAndLookResponse()
00841             response.head_succeeded = self.init_head_pose(request.camera_frame)
00842             return response
00843 
00844         if not request.have_table_centroid:
00845             response = RaiseAndLookResponse()
00846             response.head_succeeded = self.init_head_pose(request.camera_frame)
00847             self.init_spine_pose()
00848             return response
00849 
00850         # Get torso_lift_link position in base_link frame
00851         (trans, rot) = self.tf_listener.lookupTransform('base_link',
00852                                                         'torso_lift_link',
00853                                                         rospy.Time(0))
00854         lift_link_z = trans[2]
00855 
00856         # tabletop position in base_link frame
00857         request.table_centroid.header.stamp = rospy.Time(0)
00858         table_base = self.tf_listener.transformPose('base_link',
00859                                                     request.table_centroid)
00860         table_z = table_base.pose.position.z
00861         goal_lift_link_z = table_z + self.torso_z_offset
00862         lift_link_delta_z = goal_lift_link_z - lift_link_z
00863         # rospy.logdebug('Torso height (m): ' + str(lift_link_z))
00864         rospy.logdebug('Table height (m): ' + str(table_z))
00865         rospy.logdebug('Torso goal height (m): ' + str(goal_lift_link_z))
00866         # rospy.logdebug('Torso delta (m): ' + str(lift_link_delta_z))
00867 
00868         # Set goal height based on passed on table height
00869         # TODO: Set these better
00870         torso_max = 0.3
00871         torso_min = 0.01
00872         current_torso_position = np.asarray(self.robot.torso.pose()).ravel()[0]
00873         torso_goal_position = current_torso_position + lift_link_delta_z
00874         torso_goal_position = (max(min(torso_max, torso_goal_position),
00875                                    torso_min))
00876         # rospy.logdebug('Moving torso to ' + str(torso_goal_position))
00877         # Multiply by 2.0, because of units of spine
00878         self.robot.torso.set_pose(torso_goal_position)
00879 
00880         # rospy.logdebug('Got torso client result')
00881         new_torso_position = np.asarray(self.robot.torso.pose()).ravel()[0]
00882         # rospy.logdebug('New torso position is: ' + str(new_torso_position))
00883 
00884         # Get torso_lift_link position in base_link frame
00885 
00886         (new_trans, rot) = self.tf_listener.lookupTransform('base_link',
00887                                                             'torso_lift_link',
00888                                                             rospy.Time(0))
00889         new_lift_link_z = new_trans[2]
00890         # rospy.logdebug('New Torso height (m): ' + str(new_lift_link_z))
00891         # tabletop position in base_link frame
00892         new_table_base = self.tf_listener.transformPose('base_link',
00893                                                         request.table_centroid)
00894         new_table_z = new_table_base.pose.position.z
00895         rospy.loginfo('New Table height: ' + str(new_table_z))
00896 
00897         # Point the head at the table centroid
00898         # NOTE: Should we fix the tilt angle instead for consistency?
00899         look_pt = np.asmatrix([self.look_pt_x,
00900                                0.0,
00901                                -self.torso_z_offset])
00902         rospy.logdebug('Point head at ' + str(look_pt))
00903         head_res = self.robot.head.look_at(look_pt,
00904                                            request.table_centroid.header.frame_id,
00905                                            request.camera_frame)
00906         response = RaiseAndLookResponse()
00907         if head_res:
00908             rospy.loginfo('Succeeded in pointing head')
00909             response.head_succeeded = True
00910         else:
00911             rospy.logwarn('Failed to point head')
00912             response.head_succeeded = False
00913         return response
00914 
00915     #
00916     # Controller wrapper methods
00917     #
00918     def get_arm_joint_pose(self, which_arm):
00919         if which_arm == 'l':
00920             return self.robot.left.pose()
00921         else:
00922             return self.robot.right.pose()
00923 
00924     def set_arm_joint_pose(self, joint_pose, which_arm, nsecs=2.0):
00925         self.switch_to_joint_controllers()
00926         if which_arm == 'l':
00927             self.robot.left.set_pose(joint_pose, nsecs, block=True)
00928         else:
00929             self.robot.right.set_pose(joint_pose, nsecs, block=True)
00930 
00931     def move_to_cart_pose(self, pose, which_arm,
00932                           done_moving_count_thresh=None, pressure=1000):
00933         if done_moving_count_thresh is None:
00934             done_moving_count_thresh = self.arm_done_moving_count_thresh
00935         self.switch_to_cart_controllers()
00936         if which_arm == 'l':
00937             self.l_arm_cart_pub.publish(pose)
00938             posture_pub = self.l_arm_cart_posture_pub
00939             pl = self.l_pressure_listener
00940         else:
00941             self.r_arm_cart_pub.publish(pose)
00942             posture_pub = self.r_arm_cart_posture_pub
00943             pl = self.r_pressure_listener
00944 
00945         arm_not_moving_count = 0
00946         r = rospy.Rate(self.move_cart_check_hz)
00947         pl.rezero()
00948         pl.set_threshold(pressure)
00949         while arm_not_moving_count < done_moving_count_thresh:
00950             if not self.arm_moving_cart(which_arm):
00951                 arm_not_moving_count += 1
00952             else:
00953                 arm_not_moving_count = 0
00954             # Command posture
00955             m = self.get_desired_posture(which_arm)
00956             posture_pub.publish(m)
00957 
00958             if pl.check_safety_threshold():
00959                 rospy.loginfo('Exceeded pressure safety thresh!\n')
00960                 break
00961             if pl.check_threshold():
00962                 rospy.loginfo('Exceeded pressure contact thresh...')
00963                 # TODO: Let something know?
00964             r.sleep()
00965 
00966         # Return pose error
00967         if which_arm == 'l':
00968             arm_error = self.l_arm_x_err
00969         else:
00970             arm_error = self.r_arm_x_err
00971         error_dist = sqrt(arm_error.linear.x**2 + arm_error.linear.y**2 +
00972                          arm_error.linear.z**2)
00973         rospy.logdebug('Move cart gripper error dist: ' + str(error_dist)+'\n')
00974         return (arm_error, error_dist)
00975 
00976 
00977     def arm_moving_cart(self, which_arm):
00978         if which_arm == 'l':
00979             x_err = self.l_arm_x_err
00980             x_d = self.l_arm_x_d
00981         else:
00982             x_err = self.r_arm_x_err
00983             x_d = self.r_arm_x_d
00984 
00985         moving = (fabs(x_d.linear.x) > self.still_moving_velocity or
00986                   fabs(x_d.linear.y) > self.still_moving_velocity or
00987                   fabs(x_d.linear.z) > self.still_moving_velocity or
00988                   fabs(x_d.angular.x) > self.still_moving_angular_velocity or
00989                   fabs(x_d.angular.y) > self.still_moving_angular_velocity or
00990                   fabs(x_d.angular.z) > self.still_moving_angular_velocity)
00991 
00992         return moving
00993 
00994     def move_relative_gripper(self, rel_push_vector, which_arm,
00995                               move_cart_count_thresh=None, pressure=1000):
00996         rel_pose = PoseStamped()
00997         rel_pose.header.stamp = rospy.Time(0)
00998         rel_pose.header.frame_id = '/'+which_arm + '_gripper_tool_frame'
00999         rel_pose.pose.position.x = float(rel_push_vector[0])
01000         rel_pose.pose.position.y = float(rel_push_vector[1])
01001         rel_pose.pose.position.z = float(rel_push_vector[2])
01002         rel_pose.pose.orientation.x = 0
01003         rel_pose.pose.orientation.y = 0
01004         rel_pose.pose.orientation.z = 0
01005         rel_pose.pose.orientation.w = 1.0
01006         return self.move_to_cart_pose(rel_pose, which_arm,
01007                                       move_cart_count_thresh, pressure)
01008 
01009     def move_relative_torso(self, rel_push_vector, which_arm,
01010                             move_cart_count_thresh=None, pressure=1000):
01011         if which_arm == 'l':
01012             cur_pose = self.l_arm_pose
01013         else:
01014             cur_pose = self.r_arm_pose
01015         rel_pose = PoseStamped()
01016         rel_pose.header.stamp = rospy.Time(0)
01017         rel_pose.header.frame_id = 'torso_lift_link'
01018         rel_pose.pose.position.x = cur_pose.pose.position.x + \
01019             float(rel_push_vector[0])
01020         rel_pose.pose.position.y = cur_pose.pose.position.y + \
01021             float(rel_push_vector[1])
01022         rel_pose.pose.position.z = cur_pose.pose.position.z + \
01023             float(rel_push_vector[2])
01024         rel_pose.pose.orientation = cur_pose.pose.orientation
01025         return self.move_to_cart_pose(rel_pose, which_arm,
01026                                       move_cart_count_thresh, pressure)
01027 
01028     def move_relative_torso_epc(self, push_angle, push_dist, which_arm,
01029                                 move_cart_count_thresh=None, pressure=1000):
01030         delta_x = cos(push_angle)*push_dist
01031         delta_y = sin(push_angle)*push_dist
01032         move_x = cos(push_angle)
01033         move_y = cos(push_angle)
01034         if which_arm == 'l':
01035             start_pose = self.l_arm_pose
01036         else:
01037             start_pose = self.r_arm_pose
01038         desired_pose = PoseStamped()
01039         desired_pose.header.stamp = rospy.Time(0)
01040         desired_pose.header.frame_id = 'torso_lift_link'
01041         desired_pose.pose.position.x = start_pose.pose.position.x + delta_x
01042         desired_pose.pose.position.y = start_pose.pose.position.y + delta_y
01043         desired_pose.pose.position.z = start_pose.pose.position.z
01044         desired_pose.pose.orientation = start_pose.pose.orientation
01045 
01046         desired_x = desired_pose.pose.position.x
01047         desired_y = desired_pose.pose.position.y
01048 
01049         start_x = start_pose.pose.position.x
01050         start_y = start_pose.pose.position.y
01051 
01052         def move_epc_generator(cur_ep, which_arm, converged_epsilon=0.01):
01053             ep = cur_ep
01054             ep.header.stamp = rospy.Time(0)
01055             step_size = 0.001
01056             ep.pose.position.x += step_size*move_x
01057             ep.pose.position.y += step_size*move_y
01058             if which_arm == 'l':
01059                 cur_pose = self.l_arm_pose
01060             else:
01061                 cur_pose = self.r_arm_pose
01062             cur_x = cur_pose.pose.position.x
01063             cur_y = cur_pose.pose.position.y
01064             arm_error_x = desired_x - cur_x
01065             arm_error_y = desired_y - cur_y
01066             error_dist = sqrt(arm_error_x**2 + arm_error_y**2)
01067 
01068             # TODO: Check moved passed point
01069             if error_dist < converged_epsilon:
01070                 stop = 'converged'
01071             elif (((start_x > desired_x and cur_x < desired_x) or
01072                    (start_x < desired_x and cur_x > desired_x)) and
01073                   ((start_y > desired_y and cur_y < desired_y) or
01074                    (start_y < desired_y and cur_y > desired_y))):
01075                 stop = 'moved_passed'
01076             else:
01077                 stop = ''
01078             return (stop, ep)
01079 
01080         return self.move_to_cart_pose_epc(desired_pose, which_arm,
01081                                           move_epc_generator,
01082                                           move_cart_count_thresh, pressure)
01083 
01084     def move_to_cart_pose_epc(self, desired_pose, which_arm, ep_gen,
01085                               done_moving_count_thresh=None, pressure=1000,
01086                               exit_on_contact=False):
01087         if done_moving_count_thresh is None:
01088             done_moving_count_thresh = self.arm_done_moving_epc_count_thresh
01089 
01090         if which_arm == 'l':
01091             pose_pub = self.l_arm_cart_pub
01092             posture_pub = self.l_arm_cart_posture_pub
01093             pl = self.l_pressure_listener
01094         else:
01095             pose_pub = self.r_arm_cart_pub
01096             posture_pub = self.r_arm_cart_posture_pub
01097             pl = self.r_pressure_listener
01098 
01099         self.switch_to_cart_controllers()
01100 
01101         arm_not_moving_count = 0
01102         r = rospy.Rate(self.move_cart_check_hz)
01103         pl.rezero()
01104         pl.set_threshold(pressure)
01105         rospy.Time()
01106         timeout = 5
01107         timeout_at = rospy.get_time() + timeout
01108         ep = desired_pose
01109         while True:
01110             if not self.arm_moving_cart(which_arm):
01111                 arm_not_moving_count += 1
01112             else:
01113                 arm_not_moving_count = 0
01114 
01115             if arm_not_moving_count > done_moving_count_thresh:
01116                 rospy.loginfo('Exiting do to no movement!')
01117                 break
01118             if pl.check_safety_threshold():
01119                 rospy.loginfo('Exceeded pressure safety thresh!')
01120                 break
01121             if pl.check_threshold():
01122                 rospy.loginfo('Exceeded pressure contact thresh...')
01123                 if exit_on_contact:
01124                     break
01125             if timeout_at < rospy.get_time():
01126                 rospy.loginfo('Exceeded time to move EPC!')
01127                 stop = 'timed out'
01128                 break
01129             # Command posture
01130             m = self.get_desired_posture(which_arm)
01131             posture_pub.publish(m)
01132             # Command pose
01133             pose_pub.publish(ep)
01134             r.sleep()
01135 
01136             # Determine new equilibrium point
01137             stop, ep = ep_gen(ep, which_arm)
01138             if stop != '':
01139                 rospy.loginfo('Reached goal pose: ' + stop + '\n')
01140                 break
01141         self.stop_moving_cart(which_arm)
01142         # Return pose error
01143         if which_arm == 'l':
01144             arm_error = self.l_arm_x_err
01145         else:
01146             arm_error = self.r_arm_x_err
01147         error_dist = sqrt(arm_error.linear.x**2 + arm_error.linear.y**2 +
01148                           arm_error.linear.z**2)
01149         rospy.loginfo('Move cart gripper error dist: ' + str(error_dist)+'\n')
01150         return (arm_error, error_dist)
01151 
01152     def stop_moving_cart(self, which_arm):
01153         if which_arm == 'l':
01154             self.l_arm_cart_pub.publish(self.l_arm_pose)
01155         else:
01156             self.r_arm_cart_pub.publish(self.r_arm_pose)
01157 
01158     def move_down_until_contact(self, which_arm, pressure=1000):
01159         rospy.loginfo('Moving down!')
01160         down_twist = TwistStamped()
01161         down_twist.header.stamp = rospy.Time(0)
01162         down_twist.header.frame_id = 'torso_lift_link'
01163         down_twist.twist.linear.x = 0.0
01164         down_twist.twist.linear.y = 0.0
01165         down_twist.twist.linear.z = -0.1
01166         down_twist.twist.angular.x = 0.0
01167         down_twist.twist.angular.y = 0.0
01168         down_twist.twist.angular.z = 0.0
01169         return self.move_until_contact(down_twist, which_arm)
01170 
01171 
01172     def move_until_contact(self, twist, which_arm,
01173                           done_moving_count_thresh=None, pressure=1000):
01174         self.switch_to_vel_controllers()
01175         if which_arm == 'l':
01176             vel_pub = self.l_arm_cart_vel_pub
01177             posture_pub = self.l_arm_vel_posture_pub
01178             pl = self.l_pressure_listener
01179         else:
01180             vel_pub = self.r_arm_cart_vel_pub
01181             posture_pub = self.r_arm_vel_posture_pub
01182             pl = self.r_pressure_listener
01183 
01184         arm_not_moving_count = 0
01185         r = rospy.Rate(self.move_cart_check_hz)
01186         pl.rezero()
01187         pl.set_threshold(pressure)
01188         rospy.Time()
01189         timeout = 5
01190         timeout_at = rospy.get_time() + timeout
01191 
01192         while timeout_at > rospy.get_time():
01193             twist.header.stamp = rospy.Time(0)
01194             vel_pub.publish(twist)
01195             if not self.arm_moving_cart(which_arm):
01196                 arm_not_moving_count += 1
01197             else:
01198                 arm_not_moving_count = 0
01199             # Command posture
01200             m = self.get_desired_posture(which_arm)
01201             posture_pub.publish(m)
01202 
01203             if pl.check_safety_threshold():
01204                 rospy.loginfo('Exceeded pressure safety thresh!\n')
01205                 break
01206             if pl.check_threshold():
01207                 rospy.loginfo('Exceeded pressure contact thresh...')
01208                 break
01209             r.sleep()
01210 
01211         self.stop_moving_vel(which_arm)
01212 
01213         # Return pose error
01214         return
01215 
01216     def vel_push_forward(self, which_arm, speed=0.3):
01217         self.switch_to_vel_controllers()
01218 
01219         forward_twist = TwistStamped()
01220         if which_arm == 'l':
01221             vel_pub = self.l_arm_cart_vel_pub
01222             posture_pub = self.l_arm_vel_posture_pub
01223         else:
01224             vel_pub = self.r_arm_cart_vel_pub
01225             posture_pub = self.r_arm_vel_posture_pub
01226 
01227         forward_twist.header.frame_id = '/'+which_arm + '_gripper_tool_frame'
01228         forward_twist.header.stamp = rospy.Time(0)
01229         forward_twist.twist.linear.x = speed
01230         forward_twist.twist.linear.y = 0.0
01231         forward_twist.twist.linear.z = 0.0
01232         forward_twist.twist.angular.x = 0.0
01233         forward_twist.twist.angular.y = 0.0
01234         forward_twist.twist.angular.z = 0.0
01235         m = self.get_desired_posture(which_arm)
01236         posture_pub.publish(m)
01237         vel_pub.publish(forward_twist)
01238 
01239     def update_vel(self, update_twist, which_arm):
01240         # Note: assumes velocity controller already running
01241         if which_arm == 'l':
01242             vel_pub = self.l_arm_cart_vel_pub
01243             posture_pub = self.l_arm_vel_posture_pub
01244         else:
01245             vel_pub = self.r_arm_cart_vel_pub
01246             posture_pub = self.r_arm_vel_posture_pub
01247 
01248         m = self.get_desired_posture(which_arm)
01249         posture_pub.publish(m)
01250         vel_pub.publish(update_twist)
01251 
01252     def stop_moving_vel(self, which_arm):
01253         self.switch_to_vel_controllers()
01254         if which_arm == 'l':
01255             vel_pub = self.l_arm_cart_vel_pub
01256             posture_pub = self.l_arm_vel_posture_pub
01257         else:
01258             vel_pub = self.r_arm_cart_vel_pub
01259             posture_pub = self.r_arm_vel_posture_pub
01260 
01261         stop_twist = TwistStamped()
01262         stop_twist.header.stamp = rospy.Time(0)
01263         stop_twist.header.frame_id = 'torso_lift_link'
01264         stop_twist.twist.linear.x = 0.0
01265         stop_twist.twist.linear.y = 0.0
01266         stop_twist.twist.linear.z = 0.0
01267         stop_twist.twist.angular.x = 0.0
01268         stop_twist.twist.angular.y = 0.0
01269         stop_twist.twist.angular.z = 0.0
01270         vel_pub.publish(stop_twist)
01271 
01272     def get_desired_posture(self, which_arm):
01273         if which_arm == 'l':
01274             posture = 'elbowupl'
01275         else:
01276             posture = 'elbowupr'
01277 
01278         joints = self.get_arm_joint_pose(which_arm)
01279         joints = joints.tolist()
01280         joints = [j[0] for j in joints]
01281         if self.use_cur_joint_posture:
01282             m = Float64MultiArray(data=joints)
01283         else:
01284             m = Float64MultiArray(data=_POSTURES[posture])
01285         return m
01286 
01287     def l_arm_cart_state_callback(self, state_msg):
01288         x_err = state_msg.x_err
01289         x_d = state_msg.xd
01290         self.l_arm_pose = state_msg.x
01291         self.l_arm_x_err = x_err
01292         self.l_arm_x_d = x_d
01293         self.l_arm_F = state_msg.F
01294 
01295     def r_arm_cart_state_callback(self, state_msg):
01296         x_err = state_msg.x_err
01297         x_d = state_msg.xd
01298         self.r_arm_pose = state_msg.x
01299         self.r_arm_x_err = state_msg.x_err
01300         self.r_arm_x_d = state_msg.xd
01301         self.r_arm_F = state_msg.F
01302 
01303     def gripper_pose(self, which_arm="l"):
01304         if which_arm == 'l':
01305           robot_gripper = self.robot.left_gripper
01306           gripper_joint = 'l_gripper_joint'
01307         else:
01308           robot_gripper = self.robot.right_gripper
01309           gripper_joint = 'r_gripper_joint'
01310         joint_state = robot_gripper.pose(gripper_joint);
01311         print joint_state
01312 
01313     def gripper_open(self, which_arm='l'):
01314         '''
01315         Move the arm to the initial pose to be out of the way for viewing the
01316         tabletop
01317         '''
01318         if which_arm == 'l':
01319           robot_gripper = self.robot.left_gripper
01320         else:
01321           robot_gripper = self.robot.right_gripper
01322         rospy.loginfo('Opening %s_gripper' % which_arm)
01323         res = robot_gripper.open(block=True)
01324         rospy.loginfo('Opening %s_gripper' % which_arm)
01325 
01326 
01327     #
01328     # Controller setup methods
01329     #
01330     def init_joint_controllers(self):
01331         self.arm_mode = 'joint_mode'
01332         prefix = roslib.packages.get_pkg_dir('hrl_pr2_arms')+'/params/'
01333         rospy.loginfo('Setting right arm controller gains')
01334         self.cs.switch("r_arm_controller", "r_arm_controller",
01335                        prefix + "pr2_arm_controllers_push.yaml")
01336         rospy.loginfo('Setting left arm controller gains')
01337         self.cs.switch("l_arm_controller", "l_arm_controller",
01338                        prefix + "pr2_arm_controllers_push.yaml")
01339         rospy.sleep(self.post_controller_switch_sleep)
01340 
01341     def init_cart_controllers(self):
01342         self.arm_mode = 'cart_mode'
01343         if self.use_jinv:
01344             self.cs.carefree_switch('r', '%s'+self.base_cart_controller_name,
01345                                     '$(find tabletop_pushing)/params/j_inverse_params_low.yaml')
01346             self.cs.carefree_switch('l', '%s'+self.base_cart_controller_name,
01347                                     '$(find tabletop_pushing)/params/j_inverse_params_low.yaml')
01348         else:
01349             self.cs.carefree_switch('r', '%s'+self.base_cart_controller_name,
01350                                     '$(find tabletop_pushing)/params/j_transpose_task_params_pos_feedback_push.yaml')
01351             self.cs.carefree_switch('l', '%s'+self.base_cart_controller_name,
01352                                     '$(find tabletop_pushing)/params/j_transpose_task_params_pos_feedback_push.yaml')
01353 
01354         rospy.sleep(self.post_controller_switch_sleep)
01355 
01356     def init_vel_controllers(self):
01357         self.arm_mode = 'vel_mode'
01358         self.cs.carefree_switch('r', '%s'+self.base_vel_controller_name,
01359                                 '$(find tabletop_pushing)/params/j_inverse_params_custom.yaml')
01360         self.cs.carefree_switch('l', '%s'+self.base_vel_controller_name,
01361                                 '$(find tabletop_pushing)/params/j_inverse_params_custom.yaml')
01362 
01363     def switch_to_cart_controllers(self):
01364         if self.arm_mode != 'cart_mode':
01365             self.cs.carefree_switch('r', '%s'+self.base_cart_controller_name)
01366             self.cs.carefree_switch('l', '%s'+self.base_cart_controller_name)
01367             self.arm_mode = 'cart_mode'
01368             rospy.sleep(self.post_controller_switch_sleep)
01369 
01370     def switch_to_joint_controllers(self):
01371         if self.arm_mode != 'joint_mode':
01372             self.cs.carefree_switch('r', '%s_arm_controller')
01373             self.cs.carefree_switch('l', '%s_arm_controller')
01374             self.arm_mode = 'joint_mode'
01375             rospy.sleep(self.post_controller_switch_sleep)
01376 
01377     def switch_to_vel_controllers(self):
01378         if self.arm_mode != 'vel_mode':
01379             self.cs.carefree_switch('r', '%s'+self.base_vel_controller_name)
01380             self.cs.carefree_switch('l', '%s'+self.base_vel_controller_name)
01381             self.arm_mode = 'vel_mode'
01382             rospy.sleep(self.post_controller_switch_sleep)
01383 
01384     #
01385     # Main Control Loop
01386     #
01387     def run(self):
01388         '''
01389         Main control loop for the node
01390         '''
01391         self.init_spine_pose()
01392         self.init_head_pose(self.head_pose_cam_frame)
01393         self.init_arms()
01394         self.switch_to_cart_controllers()
01395         rospy.loginfo('Done initializing feedback pushing node.')
01396         rospy.spin()
01397 
01398 if __name__ == '__main__':
01399     node = PositionFeedbackPushNode()
01400     node.run()


visual_servo
Author(s): Stephan Lee
autogenerated on Wed Nov 27 2013 11:44:03