l_arm_min.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, 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 
00035 ## Gazebo tug arms for navigation
00036 
00037 PKG = 'pr2_arm_gazebo'
00038 NAME = 'l_arm_setting'
00039 
00040 import math
00041 import roslib
00042 roslib.load_manifest(PKG)
00043 roslib.load_manifest('rostest')
00044 
00045 
00046 import sys, unittest
00047 import os, os.path, threading, time
00048 import rospy, rostest
00049 from std_msgs.msg import *
00050 from pr2_mechanism_controllers.msg import *
00051 
00052 PI = 3.14159
00053 
00054 CMD_SH_PAN      = 1.0*( PI/4-1.5   ) #range [ PI/4-1.5   PI/4+1.5 ]
00055 CMD_SH_LFT      = 1.0*( -0.4       ) #range [ -0.4       1.5 ]
00056 CMD_UA_ROL      = 1.0*( 1.55-2.35 ) #range [ -1.55-2.35  -1.55+2.35 ] (+1.55 for right arm)
00057 CMD_EL_FLX      = 1.0*( -2.3       ) #range [ -2.3       0.1 ]
00058 CMD_FA_ROL      = 1.0*( 0*PI       ) #range [  ]
00059 CMD_WR_FLX      = 1.0*( -0.1       ) #range [ -0.1       2.2 ]
00060 CMD_WR_ROL      = 1.0*( 0*PI       ) #range [  ]
00061 CMD_GR_POS      = 1.0*( 0.0        ) #range [ 0          0.548 ]
00062 
00063 if __name__ == '__main__':
00064     pub_l_shoulder_pan   = rospy.Publisher("l_shoulder_pan_controller/set_command", Float64)
00065     pub_l_shoulder_lift  = rospy.Publisher("l_shoulder_lift_controller/set_command", Float64)
00066     pub_l_upper_arm_roll = rospy.Publisher("l_upper_arm_roll_controller/set_command", Float64)
00067     pub_l_elbow_flex     = rospy.Publisher("l_elbow_flex_controller/set_command", Float64)
00068     pub_l_elbow_roll     = rospy.Publisher("l_elbow_roll_controller/set_command", Float64)
00069     pub_l_wrist_flex     = rospy.Publisher("l_wrist_flex_controller/set_command", Float64)
00070     pub_l_wrist_roll     = rospy.Publisher("l_wrist_roll_controller/set_command", Float64)
00071     pub_l_gripper        = rospy.Publisher("l_gripper_controller/set_command", Float64)
00072     rospy.init_node(NAME, anonymous=True)
00073     timeout_t = time.time() + 10.0 #publish for 10 seconds then stop
00074     while time.time() < timeout_t:
00075         pub_l_shoulder_pan   .publish(Float64(CMD_SH_PAN))
00076         pub_l_shoulder_lift  .publish(Float64(CMD_SH_LFT))
00077         pub_l_upper_arm_roll .publish(Float64(CMD_UA_ROL))
00078         pub_l_elbow_flex     .publish(Float64(CMD_EL_FLX))
00079         pub_l_elbow_roll     .publish(Float64(CMD_FA_ROL))
00080         pub_l_wrist_flex     .publish(Float64(CMD_WR_FLX))
00081         pub_l_wrist_roll     .publish(Float64(CMD_WR_ROL))
00082         pub_l_gripper        .publish(Float64(CMD_GR_POS))
00083         time.sleep(0.2)
00084 


pr2_arm_gazebo
Author(s): John Hsu
autogenerated on Mon Jan 6 2014 12:02:20