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 import signal
00039 
00040 import rospy
00041 
00042 import os
00043 import sys
00044 import time
00045 import math
00046 
00047 from trajectory_msgs.msg import *
00048 from actionlib_msgs.msg import *
00049 from pr2_controllers_msgs.msg import *
00050 from pr2_common_action_msgs.msg import *
00051 import getopt
00052 import actionlib
00053 
00054 
00055 # Joint names
00056 joint_names = ["shoulder_pan", 
00057                "shoulder_lift",
00058                "upper_arm_roll",
00059                "elbow_flex", 
00060                "forearm_roll",
00061                "wrist_flex", 
00062                "wrist_roll" ]
00063 
00064 
00065 l_arm_tucked = [0.06024, 1.248526, 1.789070, -1.683386, -1.7343417, -0.0962141, -0.0864407]
00066 r_arm_tucked = [-0.023593, 1.1072800, -1.5566882, -2.124408, -1.4175, -1.8417, 0.21436]
00067 l_arm_untucked = [ 0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]
00068 r_arm_untucked = [-0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]
00069 r_arm_approach = [0.039, 1.1072, 0.0, -2.067, -1.231, -1.998, 0.369]
00070 r_arm_up_traj = [[-0.4,  0.0,   0.0,  -2.05,  0.0,  -0.1,  0.0]]
00071 
00072 # Tuck trajectory
00073 l_arm_tuck_traj = [l_arm_tucked]
00074 r_arm_tuck_traj = [r_arm_approach,
00075                    r_arm_tucked]
00076 
00077 # Untuck trajctory
00078 l_arm_untuck_traj = [l_arm_untucked]
00079 r_arm_untuck_traj = [r_arm_approach,
00080                      r_arm_untucked]
00081 
00082 # clear trajectory
00083 l_arm_clear_traj = [l_arm_untucked]
00084 r_arm_clear_traj = [r_arm_untucked]
00085 
00086 class TuckArmsActionServer:
00087   def __init__(self, node_name):
00088     self.r_received = False
00089     self.l_received = False
00090 
00091     self.node_name = node_name
00092 
00093     # arm state: -1 unknown, 0 tucked, 1 untucked
00094     self.l_arm_state = -1
00095     self.r_arm_state = -1
00096     self.success = True
00097 
00098     # Get controller name and start joint trajectory action clients
00099     self.move_duration = rospy.get_param('~move_duration', 2.5)
00100     r_action_name = rospy.get_param('~r_joint_trajectory_action', 'r_arm_controller/joint_trajectory_action')
00101     l_action_name = rospy.get_param('~l_joint_trajectory_action', 'l_arm_controller/joint_trajectory_action')
00102     self.left_joint_client = client = actionlib.SimpleActionClient(l_action_name, JointTrajectoryAction)
00103     self.right_joint_client = client = actionlib.SimpleActionClient(r_action_name, JointTrajectoryAction)
00104 
00105     # Connect to controller state
00106     rospy.Subscriber('l_arm_controller/state', JointTrajectoryControllerState ,self.stateCb)
00107     rospy.Subscriber('r_arm_controller/state', JointTrajectoryControllerState ,self.stateCb)
00108 
00109     # Wait for joint clients to connect with timeout
00110     if not self.left_joint_client.wait_for_server(rospy.Duration(30)):
00111             rospy.logerr("pr2_tuck_arms: left_joint_client action server did not come up within timelimit")
00112     if not self.right_joint_client.wait_for_server(rospy.Duration(30)):
00113             rospy.logerr("pr2_tuck_arms: right_joint_client action server did not come up within timelimit")
00114 
00115     # Construct action server
00116     self.action_server = actionlib.simple_action_server.SimpleActionServer(node_name,TuckArmsAction, self.executeCB)
00117 
00118 
00119   def executeCB(self, goal):
00120     # Make sure we received arm state
00121     while not self.r_received or not self.l_received:
00122       rospy.sleep(0.1)
00123       if rospy.is_shutdown():
00124         return
00125 
00126     # Create a new result
00127     result = TuckArmsResult()
00128     result.tuck_left = True
00129     result.tuck_right = True
00130 
00131     # Tucking left and right arm
00132     if goal.tuck_right and goal.tuck_left:
00133       rospy.loginfo('Tucking both arms...')
00134       self.tuckL()
00135       self.tuckR()
00136 
00137     # Tucking left arm, untucking right arm
00138     elif goal.tuck_left and not goal.tuck_right:
00139       rospy.loginfo('Tucking left arm and untucking right arm...')
00140       self.untuckR()
00141       self.tuckL()
00142 
00143     # Tucking right arm, untucking left arm
00144     if goal.tuck_right and not goal.tuck_left:
00145       rospy.loginfo('Tucking right arm and untucking left arm...')        
00146       self.untuckL()
00147       self.tuckR()
00148 
00149     # UnTucking both arms
00150     if not goal.tuck_right and not goal.tuck_left:
00151       rospy.loginfo("Untucking both arms...")
00152       self.untuckL()
00153       self.untuckR()
00154 
00155     # Succeed or fail
00156     if self.success:
00157       result.tuck_right = goal.tuck_right
00158       result.tuck_left = goal.tuck_left
00159       self.action_server.set_succeeded(result)
00160     else:
00161       rospy.logerr("Tuck or untuck arms FAILED: Right value: %d. Left value: %d" % (result.tuck_left, result.tuck_right))
00162       result.tuck_right = (self.r_arm_state == 0)
00163       result.tuck_left = (self.l_arm_state == 0)
00164       self.action_server.set_aborted(result)
00165 
00166 
00167   # clears r arm and l arm
00168   def tuckL(self):
00169     if self.l_arm_state != 0:
00170       self.go('r', r_arm_up_traj)
00171       if self.l_arm_state != 1:
00172         self.go('l', l_arm_clear_traj)
00173       self.go('l', l_arm_tuck_traj)
00174       self.go('r', r_arm_clear_traj)
00175     
00176   # clears r arm
00177   def untuckL(self):
00178     if self.l_arm_state != 1:
00179       self.go('r', r_arm_up_traj)
00180       if self.l_arm_state == 0:
00181         self.go('l', l_arm_untuck_traj)
00182       elif self.l_arm_state == -1:
00183         self.go('l', l_arm_clear_traj)
00184 
00185   # assumes l tucked or cleared
00186   def tuckR(self):
00187     if self.r_arm_state != 0:
00188       self.go('r', r_arm_tuck_traj)
00189 
00190   # assumes l tucked or cleared
00191   def untuckR(self):
00192     if self.r_arm_state == 0:
00193       self.go('r', r_arm_untuck_traj)
00194     elif self.r_arm_state == -1:
00195       self.go('r', r_arm_clear_traj)
00196 
00197   def go(self, side, positions, wait = True):
00198     goal = JointTrajectoryGoal()
00199     goal.trajectory.joint_names = [side+"_"+name+"_joint" for name in joint_names]
00200     goal.trajectory.points = []
00201     for p, count in zip(positions, range(0,len(positions)+1)):
00202       goal.trajectory.points.append(JointTrajectoryPoint( positions = p,
00203                                                           velocities = [],
00204                                                           accelerations = [],
00205                                                           time_from_start = rospy.Duration((count+1) * self.move_duration)))
00206     goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
00207     if wait:
00208       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)):
00209         self.success = False
00210     else:
00211       {'l': self.left_joint_client, 'r': self.right_joint_client}[side].send_goal(goal)
00212 
00213 
00214   # Returns angle between -pi and + pi
00215   def angleWrap(self, angle):
00216     while angle > math.pi:
00217       angle -= math.pi*2.0
00218     while angle < -math.pi:
00219       angle += math.pi*2.0
00220     return angle
00221 
00222 
00223   def stateCb(self, msg):
00224     l_sum_tucked = 0
00225     l_sum_untucked = 0
00226     r_sum_tucked = 0
00227     r_sum_untucked = 0
00228     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):
00229       value_state = self.angleWrap(value_state)
00230 
00231       if 'l_'+name_desi+'_joint' == name_state:
00232         self.l_received = True
00233         l_sum_tucked = l_sum_tucked + math.fabs(value_state - value_l_tucked)
00234         l_sum_untucked = l_sum_untucked + math.fabs(value_state - value_l_untucked)
00235       if 'r_'+name_desi+'_joint' == name_state:
00236         self.r_received = True
00237         r_sum_tucked = r_sum_tucked + math.fabs(value_state - value_r_tucked)
00238         r_sum_untucked = r_sum_untucked + math.fabs(value_state - value_r_untucked)
00239 
00240     if l_sum_tucked > 0 and l_sum_tucked < 0.1:
00241       self.l_arm_state = 0
00242     elif l_sum_untucked > 0 and l_sum_untucked < 0.3:
00243       self.l_arm_state = 1
00244     elif l_sum_tucked >= 0.1 and l_sum_untucked >= 0.3:
00245       self.l_arm_state = -1    
00246 
00247     if r_sum_tucked > 0 and r_sum_tucked < 0.1:
00248       self.r_arm_state = 0
00249     elif r_sum_untucked > 0 and r_sum_untucked < 0.3:
00250       self.r_arm_state = 1
00251     elif r_sum_tucked >= 0.1 and r_sum_untucked >= 0.3:
00252       self.r_arm_state = -1    
00253 
00254 
00255 def main():
00256   action_name = 'tuck_arms'
00257   rospy.init_node(action_name)
00258   rospy.sleep(0.001)  # wait for time
00259   tuck_arms_action_server = TuckArmsActionServer(action_name)
00260 
00261   rospy.spin()
00262 
00263 
00264 if __name__ == '__main__':
00265   main()


pr2_tuck_arms_action
Author(s): Wim Meeussen
autogenerated on Thu Jun 6 2019 17:52:32