test_arm.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 test arm controller
00036 ##   sends posision
00037 ##   checks to see if P3D returns corresponding ground truth within TARGET_TOL of TARGET_VW
00038 ##          for a duration of TARGET_DURATION seconds
00039 
00040 PKG = 'pr2_gazebo'
00041 NAME = 'test_arm'
00042 
00043 import math
00044 import roslib
00045 roslib.load_manifest(PKG)
00046 
00047 import sys, unittest
00048 import os, os.path, threading, time
00049 import rospy, rostest
00050 from std_msgs.msg import *
00051 from nav_msgs.msg import *
00052 from pr2_mechanism_controllers.msg import *
00053 from tf.transformations import *
00054 from numpy import *
00055 
00056 
00057 GRP_CMD_POS     = 0.03
00058 
00059 TARGET_DURATION = 1.0
00060 ROT_TARGET_TOL      = 0.01  #empirical test result john - 20090110
00061 POS_TARGET_TOL      = 0.01  #empirical test result john - 20090110
00062 TEST_TIMEOUT    = 100.0
00063 
00064 # pre-recorded poses for above commands
00065 TARGET_PALM_TX     = 0.118296477266
00066 TARGET_PALM_TY     = -0.113566972556
00067 TARGET_PALM_TZ     = 0.429595972393
00068 TARGET_PALM_QX     = -0.478307662414
00069 TARGET_PALM_QY     = 0.491088172771
00070 TARGET_PALM_QZ     = -0.547883729434
00071 TARGET_PALM_QW     = 0.479455530433
00072 
00073 TARGET_FNGR_TX     =  0.112382617544
00074 TARGET_FNGR_TY     =  -0.190916084688
00075 TARGET_FNGR_TZ     =  0.378231687397
00076 TARGET_FNGR_QX     =  -0.436046001411
00077 TARGET_FNGR_QY     =  0.528981109854
00078 TARGET_FNGR_QZ     =  -0.506382999652
00079 TARGET_FNGR_QW     =  0.523086157085
00080 
00081 class ArmTest(unittest.TestCase):
00082     def __init__(self, *args):
00083         super(ArmTest, self).__init__(*args)
00084         self.palm_success = False
00085         self.reached_target_palm = False
00086         self.duration_start_palm = 0
00087         self.fngr_success = False
00088         self.reached_target_fngr = False
00089         self.duration_start_fngr = 0
00090 
00091 
00092     def printP3D(self, p3d):
00093         print "pose ground truth received"
00094         print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
00095         print "                   " + "y: " + str(p3d.pos.position.y)
00096         print "                   " + "z: " + str(p3d.pose.pose.position.z)
00097         print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
00098         print "                   " + "y: " + str(p3d.pose.pose.orientation.y)
00099         print "                   " + "z: " + str(p3d.pose.pose.orientation.z)
00100         print "                   " + "w: " + str(p3d.pose.pose.orientation.w)
00101         print "P3D rate translan: " + "x: " + str(p3d.vel.vel.vx)
00102         print "                   " + "y: " + str(p3d.vel.vel.vy)
00103         print "                   " + "z: " + str(p3d.vel.vel.vz)
00104         print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
00105         print "                   " + "y: " + str(p3d.vel.ang_vel.vy)
00106         print "                   " + "z: " + str(p3d.vel.ang_vel.vz)
00107 
00108     def fngrP3dInput(self, p3d):
00109         i = 0
00110         pos_error = abs(p3d.pose.pose.position.x - TARGET_FNGR_TX) + \
00111                     abs(p3d.pose.pose.position.y - TARGET_FNGR_TY) + \
00112                     abs(p3d.pose.pose.position.z - TARGET_FNGR_TZ)
00113 
00114         #target pose rotation matrix
00115         target_q = [TARGET_FNGR_QX  \
00116                    ,TARGET_FNGR_QY  \
00117                    ,TARGET_FNGR_QZ  \
00118                    ,TARGET_FNGR_QW]
00119 
00120         #p3d pose quaternion
00121         p3d_q     = [p3d.pose.pose.orientation.x  \
00122                     ,p3d.pose.pose.orientation.y  \
00123                     ,p3d.pose.pose.orientation.z  \
00124                     ,p3d.pose.pose.orientation.w]
00125 
00126         # get error euler angles by inverting the target rotation matrix and multiplying by p3d quaternion
00127         target_q_inv = quaternion_inverse(target_q)
00128         rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
00129         rot_error = abs( rot_euler[0] ) + \
00130                     abs( rot_euler[1] ) + \
00131                     abs( rot_euler[2] )
00132 
00133         print " fngr Error pos: " + str(pos_error) + " rot: " + str(rot_error)
00134 
00135         #self.printP3D(p3d) #for getting new valid data
00136 
00137         # has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
00138         if self.reached_target_fngr:
00139           print " fngr duration: " + str(time.time() - self.duration_start_fngr)
00140           if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
00141             if time.time() - self.duration_start_fngr > TARGET_DURATION:
00142               self.fngr_success = True
00143           else:
00144             # failed to maintain target vw, reset duration
00145             self.fngr_success = False
00146             self.reached_target_fngr = False
00147         else:
00148           if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
00149             print 'success finger'
00150             self.reached_target_fngr = True
00151             self.duration_start_fngr = time.time()
00152 
00153     def palmP3dInput(self, p3d):
00154         i = 0
00155         pos_error = abs(p3d.pose.pose.position.x - TARGET_PALM_TX) + \
00156                     abs(p3d.pose.pose.position.y - TARGET_PALM_TY) + \
00157                     abs(p3d.pose.pose.position.z - TARGET_PALM_TZ)
00158 
00159         #target pose rotation matrix
00160         target_q = [TARGET_PALM_QX  \
00161                    ,TARGET_PALM_QY  \
00162                    ,TARGET_PALM_QZ  \
00163                    ,TARGET_PALM_QW]
00164 
00165         #p3d pose quaternion
00166         p3d_q     = [p3d.pose.pose.orientation.x  \
00167                     ,p3d.pose.pose.orientation.y  \
00168                     ,p3d.pose.pose.orientation.z  \
00169                     ,p3d.pose.pose.orientation.w]
00170 
00171         # get error euler angles by inverting the target rotation matrix and multiplying by p3d quaternion
00172         target_q_inv = quaternion_inverse(target_q)
00173         rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
00174         rot_error = abs( rot_euler[0] ) + \
00175                     abs( rot_euler[1] ) + \
00176                     abs( rot_euler[2] )
00177 
00178         print " palm Error pos: " + str(pos_error) + " rot: " + str(rot_error)
00179 
00180         #self.printP3D(p3d) #for getting new valid data
00181 
00182         # has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
00183         if self.reached_target_palm: # tracking started
00184           print " palm duration: " + str(time.time() - self.duration_start_palm)
00185           if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
00186             if time.time() - self.duration_start_palm > TARGET_DURATION:
00187               self.palm_success = True
00188           else:
00189             # failed to maintain target vw, reset duration
00190             self.palm_success = False
00191             self.reached_target_palm = False
00192         else:
00193           if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
00194             print 'success palm'
00195             self.reached_target_palm = True
00196             self.duration_start_palm = time.time()
00197     
00198     def test_arm(self):
00199         print "LNK\n"
00200         pub_gripper = rospy.Publisher("/l_gripper_position_controller/command", Float64)
00201         rospy.Subscriber("/l_gripper_palm_pose_ground_truth", Odometry, self.palmP3dInput)
00202         rospy.Subscriber("/l_gripper_l_finger_pose_ground_truth", Odometry, self.fngrP3dInput)
00203         rospy.init_node(NAME, anonymous=True)
00204         timeout_t = time.time() + TEST_TIMEOUT
00205 
00206         while not rospy.is_shutdown() and (not self.palm_success or not self.fngr_success) and time.time() < timeout_t:
00207             pub_gripper.publish(Float64(GRP_CMD_POS))
00208             time.sleep(1.0)
00209 
00210         if not (self.palm_success and self.fngr_success):
00211           rospy.logerr("finger and palm pose test failed, there could be a problem with the gazebo_ros_p3d controller, tuck position has changed, or the gripper controller failed to open the gripper to 3cm width.")
00212 
00213         self.assert_(self.palm_success and self.fngr_success)
00214 
00215 if __name__ == '__main__':
00216     rostest.run(PKG, sys.argv[0], ArmTest, sys.argv) #, text_mode=True)


pr2_gazebo
Author(s): John Hsu
autogenerated on Sat Sep 5 2015 11:23:01