collision_free_arm_cmder.py
Go to the documentation of this file.00001 #! /usr/bin/env python 00002 # 00003 # Copyright (c) 2010, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions are met: 00008 # 00009 # * Redistributions of source code must retain the above copyright 00010 # notice, this list of conditions and the following disclaimer. 00011 # * Redistributions in binary form must reproduce the above copyright 00012 # notice, this list of conditions and the following disclaimer in the 00013 # documentation and/or other materials provided with the distribution. 00014 # * Neither the name of the Willow Garage, Inc. nor the names of its 00015 # contributors may be used to endorse or promote products derived from 00016 # this software without specific prior written permission. 00017 # 00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 # POSSIBILITY OF SUCH DAMAGE. 00029 00030 ##\author Kevin Watts 00031 ##\brief Sends goals to arm to move it in collision free way 00032 00033 PKG = 'life_test' 00034 import roslib; roslib.load_manifest(PKG) 00035 import rospy 00036 import actionlib 00037 from life_test.commanders.arm_cmder import ArmCmder 00038 00039 from pr2_controllers_msgs.msg import JointTrajectoryAction 00040 00041 arm_ranges = { 00042 'r_shoulder_pan_joint': (-2.0, 0.4), 00043 'r_shoulder_lift_joint': (-0.4, 1.25), 00044 'r_upper_arm_roll_joint': (-3.5, 0.3), 00045 'r_elbow_flex_joint': (-2.0, -0.05) 00046 } 00047 00048 recovery_positions = { 00049 'r_shoulder_pan_joint': -0.8, 00050 'r_shoulder_lift_joint': 0.3, 00051 'r_upper_arm_roll_joint': 0.0, 00052 'r_elbow_flex_joint': -0.2 00053 } 00054 00055 if __name__ == '__main__': 00056 rospy.init_node('arm_cmder_client') 00057 client = actionlib.SimpleActionClient('collision_free_arm_trajectory_action_right_arm', 00058 JointTrajectoryAction) 00059 rospy.loginfo('Waiting for server for collision free arm commander') 00060 client.wait_for_server() 00061 rospy.loginfo('Sending arm commands') 00062 00063 my_rate = rospy.Rate(1.0) 00064 00065 recovery_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_action', 00066 JointTrajectoryAction) 00067 00068 cmder = ArmCmder(client, arm_ranges, recovery_client, recovery_positions) 00069 00070 while not rospy.is_shutdown(): 00071 cmder.send_cmd() 00072 my_rate.sleep()