tuck_arms_main.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
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 Willow Garage nor the names of its
00018 #    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 # Author: Wim Meeussen
00035 # Modified by Jonathan Bohren to be an action and for untucking
00036 
00037 
00038 """
00039 usage: tuck_arms.py [-q] [-l ACTION] [-r ACTION]
00040 
00041 Options:
00042   -q or --quit   Quit when finished
00043   -l or --left   Action for left arm
00044   -r or --right  Action for right arm
00045 
00046 Actions:
00047   t or tuck
00048   u or untuck
00049 
00050 NB: If two arms are specified, actions must be the same for both arms.
00051 
00052 """
00053 
00054 import roslib
00055 import signal
00056 roslib.load_manifest('pr2_tuck_arms_action')
00057 
00058 import rospy
00059 
00060 import os
00061 import sys
00062 import time
00063 import math
00064 
00065 from trajectory_msgs.msg import *
00066 from actionlib_msgs.msg import *
00067 from pr2_controllers_msgs.msg import *
00068 from pr2_common_action_msgs.msg import *
00069 import getopt
00070 import actionlib
00071 
00072 def usage():
00073   print __doc__ % vars()
00074   rospy.signal_shutdown("Help requested")
00075 
00076 # Joint names
00077 joint_names = ["shoulder_pan", 
00078                "shoulder_lift",
00079                "upper_arm_roll",
00080                "elbow_flex", 
00081                "forearm_roll",
00082                "wrist_flex", 
00083                "wrist_roll" ]
00084 
00085 
00086 l_arm_tucked = [0.06024, 1.248526, 1.789070, -1.683386, -1.7343417, -0.0962141, -0.0864407]
00087 r_arm_tucked = [-0.023593, 1.1072800, -1.5566882, -2.124408, -1.4175, -1.8417, 0.21436]
00088 l_arm_untucked = [ 0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]
00089 r_arm_untucked = [-0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]
00090 r_arm_approach = [0.039, 1.1072, 0.0, -2.067, -1.231, -1.998, 0.369]
00091 r_arm_up_traj = [[-0.4,  0.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]]
00092 
00093 # Tuck trajectory
00094 l_arm_tuck_traj = [l_arm_tucked]
00095 r_arm_tuck_traj = [r_arm_approach,
00096                    r_arm_tucked]
00097 
00098 # Untuck trajctory
00099 l_arm_untuck_traj = [l_arm_untucked]
00100 r_arm_untuck_traj = [r_arm_approach,
00101                      r_arm_untucked]
00102 
00103 # clear trajectory
00104 l_arm_clear_traj = [l_arm_untucked]
00105 r_arm_clear_traj = [r_arm_untucked]
00106 
00107 class TuckArmsActionServer:
00108   def __init__(self, node_name):
00109     self.r_received = False
00110     self.l_received = False
00111 
00112     self.node_name = node_name
00113 
00114     # arm state: -1 unknown, 0 tucked, 1 untucked
00115     self.l_arm_state = -1
00116     self.r_arm_state = -1
00117     self.success = True
00118 
00119     # Get controller name and start joint trajectory action clients
00120     self.move_duration = rospy.get_param('~move_duration', 2.5)
00121     r_action_name = rospy.get_param('~r_joint_trajectory_action', 'r_arm_controller/joint_trajectory_action')
00122     l_action_name = rospy.get_param('~l_joint_trajectory_action', 'l_arm_controller/joint_trajectory_action')
00123     self.left_joint_client = client = actionlib.SimpleActionClient(l_action_name, JointTrajectoryAction)
00124     self.right_joint_client = client = actionlib.SimpleActionClient(r_action_name, JointTrajectoryAction)
00125 
00126     # Connect to controller state
00127     rospy.Subscriber('l_arm_controller/state', JointTrajectoryControllerState ,self.stateCb)
00128     rospy.Subscriber('r_arm_controller/state', JointTrajectoryControllerState ,self.stateCb)
00129 
00130     # Wait for joint clients to connect with timeout
00131     if not self.left_joint_client.wait_for_server(rospy.Duration(30)):
00132             rospy.logerr("pr2_tuck_arms: left_joint_client action server did not come up within timelimit")
00133     if not self.right_joint_client.wait_for_server(rospy.Duration(30)):
00134             rospy.logerr("pr2_tuck_arms: right_joint_client action server did not come up within timelimit")
00135 
00136     # Construct action server
00137     self.action_server = actionlib.simple_action_server.SimpleActionServer(node_name,TuckArmsAction, self.executeCB)
00138 
00139 
00140   def executeCB(self, goal):
00141     # Make sure we received arm state
00142     while not self.r_received or not self.l_received:
00143       rospy.sleep(0.1)
00144       if rospy.is_shutdown():
00145         return
00146 
00147     # Create a new result
00148     result = TuckArmsResult()
00149     result.tuck_left = True
00150     result.tuck_right = True
00151 
00152     # Tucking left and right arm
00153     if goal.tuck_right and goal.tuck_left:
00154       rospy.loginfo('Tucking both arms...')
00155       self.tuckL()
00156       self.tuckR()
00157 
00158     # Tucking left arm, untucking right arm
00159     elif goal.tuck_left and not goal.tuck_right:
00160       rospy.loginfo('Tucking left arm and untucking right arm...')
00161       self.untuckR()
00162       self.tuckL()
00163 
00164     # Tucking right arm, untucking left arm
00165     if goal.tuck_right and not goal.tuck_left:
00166       rospy.loginfo('Tucking right arm and untucking left arm...')        
00167       self.untuckL()
00168       self.tuckR()
00169 
00170     # UnTucking both arms
00171     if not goal.tuck_right and not goal.tuck_left:
00172       rospy.loginfo("Untucking both arms...")
00173       self.untuckL()
00174       self.untuckR()
00175 
00176     # Succeed or fail
00177     if self.success:
00178       result.tuck_right = goal.tuck_right
00179       result.tuck_left = goal.tuck_left
00180       self.action_server.set_succeeded(result)
00181     else:
00182       rospy.logerr("Tuck or untuck arms FAILED: Right value: %d. Left value: %d" % (result.tuck_left, result.tuck_right))
00183       result.tuck_right = (self.r_arm_state == 0)
00184       result.tuck_left = (self.l_arm_state == 0)
00185       self.action_server.set_aborted(result)
00186 
00187 
00188   # clears r arm and l arm
00189   def tuckL(self):
00190     if self.l_arm_state != 0:
00191       self.go('r', r_arm_up_traj)
00192       if self.l_arm_state != 1:
00193         self.go('l', l_arm_clear_traj)
00194       self.go('l', l_arm_tuck_traj)
00195       self.go('r', r_arm_clear_traj)
00196     
00197   # clears r arm
00198   def untuckL(self):
00199     if self.l_arm_state != 1:
00200       self.go('r', r_arm_up_traj)
00201       if self.l_arm_state == 0:
00202         self.go('l', l_arm_untuck_traj)
00203       elif self.l_arm_state == -1:
00204         self.go('l', l_arm_clear_traj)
00205 
00206   # assumes l tucked or cleared
00207   def tuckR(self):
00208     if self.r_arm_state != 0:
00209       self.go('r', r_arm_tuck_traj)
00210 
00211   # assumes l tucked or cleared
00212   def untuckR(self):
00213     if self.r_arm_state == 0:
00214       self.go('r', r_arm_untuck_traj)
00215     elif self.r_arm_state == -1:
00216       self.go('r', r_arm_clear_traj)
00217 
00218   def go(self, side, positions, wait = True):
00219     goal = JointTrajectoryGoal()
00220     goal.trajectory.joint_names = [side+"_"+name+"_joint" for name in joint_names]
00221     goal.trajectory.points = []
00222     for p, count in zip(positions, range(0,len(positions)+1)):
00223       goal.trajectory.points.append(JointTrajectoryPoint( positions = p,
00224                                                           velocities = [],
00225                                                           accelerations = [],
00226                                                           time_from_start = rospy.Duration((count+1) * self.move_duration)))
00227     goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
00228     if wait:
00229       if not {'l': self.left_joint_client, 'r': self.right_joint_client}[side].send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0)):
00230         self.success = False
00231     else:
00232       {'l': self.left_joint_client, 'r': self.right_joint_client}[side].send_goal(goal)
00233 
00234 
00235   # Returns angle between -pi and + pi
00236   def angleWrap(self, angle):
00237     while angle > math.pi:
00238       angle -= math.pi*2.0
00239     while angle < -math.pi:
00240       angle += math.pi*2.0
00241     return angle
00242 
00243 
00244   def stateCb(self, msg):
00245     l_sum_tucked = 0
00246     l_sum_untucked = 0
00247     r_sum_tucked = 0
00248     r_sum_untucked = 0
00249     for name_state, name_desi, value_state, value_l_tucked, value_l_untucked, value_r_tucked, value_r_untucked in zip(msg.joint_names, joint_names, msg.actual.positions , l_arm_tucked, l_arm_untucked, r_arm_tucked, r_arm_untucked):
00250       value_state = self.angleWrap(value_state)
00251 
00252       if 'l_'+name_desi+'_joint' == name_state:
00253         self.l_received = True
00254         l_sum_tucked = l_sum_tucked + math.fabs(value_state - value_l_tucked)
00255         l_sum_untucked = l_sum_untucked + math.fabs(value_state - value_l_untucked)
00256       if 'r_'+name_desi+'_joint' == name_state:
00257         self.r_received = True
00258         r_sum_tucked = r_sum_tucked + math.fabs(value_state - value_r_tucked)
00259         r_sum_untucked = r_sum_untucked + math.fabs(value_state - value_r_untucked)
00260 
00261     if l_sum_tucked > 0 and l_sum_tucked < 0.1:
00262       self.l_arm_state = 0
00263     elif l_sum_untucked > 0 and l_sum_untucked < 0.3:
00264       self.l_arm_state = 1
00265     elif l_sum_tucked >= 0.1 and l_sum_untucked >= 0.3:
00266       self.l_arm_state = -1    
00267 
00268     if r_sum_tucked > 0 and r_sum_tucked < 0.1:
00269       self.r_arm_state = 0
00270     elif r_sum_untucked > 0 and r_sum_untucked < 0.3:
00271       self.r_arm_state = 1
00272     elif r_sum_tucked >= 0.1 and r_sum_untucked >= 0.3:
00273       self.r_arm_state = -1    
00274 
00275 if __name__ == '__main__':
00276   main()
00277 
00278 
00279 def main():
00280   action_name = 'tuck_arms'
00281   rospy.init_node(action_name)
00282   rospy.sleep(0.001)  # wait for time
00283   tuck_arms_action_server = TuckArmsActionServer(action_name)
00284 
00285   quit_when_finished = False
00286 
00287   # check for command line arguments, and send goal to action server if required
00288   myargs = rospy.myargv()[1:]
00289   if len(myargs):
00290     goal = TuckArmsGoal()
00291     goal.tuck_left = True
00292     goal.tuck_right = True
00293     opts, args = getopt.getopt(myargs, 'hql:r:', ['quit','left','right'])
00294     for arm, action in opts:
00295       
00296       if arm in ('-l', '--left'):
00297         if action in ('tuck', 't'):
00298           goal.tuck_left = True
00299         elif action in ('untuck', 'u'):
00300           goal.tuck_left = False
00301         else:
00302            rospy.logerr('Invalid action for right arm: %s'%action)
00303            rospy.signal_shutdown("ERROR")
00304 
00305       if arm in ('-r', '--right'):
00306         if action in ('tuck', 't'):
00307           goal.tuck_right = True
00308         elif action in ('untuck', 'u'):
00309           goal.tuck_right = False
00310         else:
00311            rospy.logerr('Invalid action for left arm: %s'%action)
00312            rospy.signal_shutdown("ERROR")
00313 
00314       if arm in ('--quit', '-q'):
00315         quit_when_finished = True
00316 
00317       if arm in ('--help', '-h'):
00318         usage()
00319     
00320     tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
00321     rospy.logdebug('Waiting for action server to start')
00322     tuck_arm_client.wait_for_server(rospy.Duration(10.0))
00323     rospy.logdebug('Sending goal to action server')
00324     tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
00325 
00326     if quit_when_finished:
00327       rospy.signal_shutdown("Quitting")
00328 
00329   rospy.spin()
00330 


pr2_tuck_arms_action
Author(s): Wim Meeussen
autogenerated on Tue Apr 22 2014 19:31:21