test_arm.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the Willow Garage nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 
35 ## Gazebo test arm controller
36 ## sends posision
37 ## checks to see if P3D returns corresponding ground truth within TARGET_TOL of TARGET_VW
38 ## for a duration of TARGET_DURATION seconds
39 
40 PKG = 'pr2_gazebo'
41 NAME = 'test_arm'
42 
43 import math
44 import roslib
45 roslib.load_manifest(PKG)
46 
47 import sys, unittest
48 import os, os.path, threading, time
49 import rospy, rostest
50 from std_msgs.msg import *
51 from nav_msgs.msg import *
53 from tf.transformations import *
54 from numpy import *
55 
56 
57 GRP_CMD_POS = 0.03
58 
59 TARGET_DURATION = 1.0
60 ROT_TARGET_TOL = 0.01 #empirical test result john - 20090110
61 POS_TARGET_TOL = 0.01 #empirical test result john - 20090110
62 TEST_TIMEOUT = 100.0
63 
64 # pre-recorded poses for above commands
65 TARGET_PALM_TX = 0.118296477266
66 TARGET_PALM_TY = -0.113566972556
67 TARGET_PALM_TZ = 0.429595972393
68 TARGET_PALM_QX = -0.478307662414
69 TARGET_PALM_QY = 0.491088172771
70 TARGET_PALM_QZ = -0.547883729434
71 TARGET_PALM_QW = 0.479455530433
72 
73 TARGET_FNGR_TX = 0.112382617544
74 TARGET_FNGR_TY = -0.190916084688
75 TARGET_FNGR_TZ = 0.378231687397
76 TARGET_FNGR_QX = -0.436046001411
77 TARGET_FNGR_QY = 0.528981109854
78 TARGET_FNGR_QZ = -0.506382999652
79 TARGET_FNGR_QW = 0.523086157085
80 
81 class ArmTest(unittest.TestCase):
82  def __init__(self, *args):
83  super(ArmTest, self).__init__(*args)
84  self.palm_success = False
85  self.reached_target_palm = False
87  self.fngr_success = False
88  self.reached_target_fngr = False
90 
91 
92  def printP3D(self, p3d):
93  print "pose ground truth received"
94  print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x)
95  print " " + "y: " + str(p3d.pose.pose.position.y)
96  print " " + "z: " + str(p3d.pose.pose.position.z)
97  print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x)
98  print " " + "y: " + str(p3d.pose.pose.orientation.y)
99  print " " + "z: " + str(p3d.pose.pose.orientation.z)
100  print " " + "w: " + str(p3d.pose.pose.orientation.w)
101  print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x)
102  print " " + "y: " + str(p3d.twist.twist.linear.y)
103  print " " + "z: " + str(p3d.twist.twist.linear.z)
104  print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.x)
105  print " " + "y: " + str(p3d.twist.twist.angular.y)
106  print " " + "z: " + str(p3d.twist.twist.angular.z)
107 
108  def fngrP3dInput(self, p3d):
109  i = 0
110  pos_error = abs(p3d.pose.pose.position.x - TARGET_FNGR_TX) + \
111  abs(p3d.pose.pose.position.y - TARGET_FNGR_TY) + \
112  abs(p3d.pose.pose.position.z - TARGET_FNGR_TZ)
113 
114  #target pose rotation matrix
115  target_q = [TARGET_FNGR_QX \
116  ,TARGET_FNGR_QY \
117  ,TARGET_FNGR_QZ \
118  ,TARGET_FNGR_QW]
119 
120  #p3d pose quaternion
121  p3d_q = [p3d.pose.pose.orientation.x \
122  ,p3d.pose.pose.orientation.y \
123  ,p3d.pose.pose.orientation.z \
124  ,p3d.pose.pose.orientation.w]
125 
126  # get error euler angles by inverting the target rotation matrix and multiplying by p3d quaternion
127  target_q_inv = quaternion_inverse(target_q)
128  rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
129  rot_error = abs( rot_euler[0] ) + \
130  abs( rot_euler[1] ) + \
131  abs( rot_euler[2] )
132 
133  print " fngr Error pos: " + str(pos_error) + " rot: " + str(rot_error)
134 
135  #self.printP3D(p3d) #for getting new valid data
136 
137  # has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
138  if self.reached_target_fngr:
139  print " fngr duration: " + str(time.time() - self.duration_start_fngr)
140  if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
141  if time.time() - self.duration_start_fngr > TARGET_DURATION:
142  self.fngr_success = True
143  else:
144  # failed to maintain target vw, reset duration
145  self.fngr_success = False
146  self.reached_target_fngr = False
147  else:
148  if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
149  print 'success finger'
150  self.reached_target_fngr = True
151  self.duration_start_fngr = time.time()
152 
153  def palmP3dInput(self, p3d):
154  i = 0
155  pos_error = abs(p3d.pose.pose.position.x - TARGET_PALM_TX) + \
156  abs(p3d.pose.pose.position.y - TARGET_PALM_TY) + \
157  abs(p3d.pose.pose.position.z - TARGET_PALM_TZ)
158 
159  #target pose rotation matrix
160  target_q = [TARGET_PALM_QX \
161  ,TARGET_PALM_QY \
162  ,TARGET_PALM_QZ \
163  ,TARGET_PALM_QW]
164 
165  #p3d pose quaternion
166  p3d_q = [p3d.pose.pose.orientation.x \
167  ,p3d.pose.pose.orientation.y \
168  ,p3d.pose.pose.orientation.z \
169  ,p3d.pose.pose.orientation.w]
170 
171  # get error euler angles by inverting the target rotation matrix and multiplying by p3d quaternion
172  target_q_inv = quaternion_inverse(target_q)
173  rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
174  rot_error = abs( rot_euler[0] ) + \
175  abs( rot_euler[1] ) + \
176  abs( rot_euler[2] )
177 
178  print " palm Error pos: " + str(pos_error) + " rot: " + str(rot_error)
179 
180  #self.printP3D(p3d) #for getting new valid data
181 
182  # has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
183  if self.reached_target_palm: # tracking started
184  print " palm duration: " + str(time.time() - self.duration_start_palm)
185  if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
186  if time.time() - self.duration_start_palm > TARGET_DURATION:
187  self.palm_success = True
188  else:
189  # failed to maintain target vw, reset duration
190  self.palm_success = False
191  self.reached_target_palm = False
192  else:
193  if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
194  print 'success palm'
195  self.reached_target_palm = True
196  self.duration_start_palm = time.time()
197 
198  def test_arm(self):
199  print "LNK\n"
200  pub_gripper = rospy.Publisher("/l_gripper_position_controller/command", Float64)
201  rospy.Subscriber("/l_gripper_palm_pose_ground_truth", Odometry, self.palmP3dInput)
202  rospy.Subscriber("/l_gripper_l_finger_pose_ground_truth", Odometry, self.fngrP3dInput)
203  rospy.init_node(NAME, anonymous=True)
204  timeout_t = time.time() + TEST_TIMEOUT
205 
206  while not rospy.is_shutdown() and (not self.palm_success or not self.fngr_success) and time.time() < timeout_t:
207  pub_gripper.publish(Float64(GRP_CMD_POS))
208  time.sleep(1.0)
209 
210  if not (self.palm_success and self.fngr_success):
211  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.")
212 
213  self.assert_(self.palm_success and self.fngr_success)
214 
215 if __name__ == '__main__':
216  rostest.run(PKG, sys.argv[0], ArmTest, sys.argv) #, text_mode=True)
def fngrP3dInput(self, p3d)
Definition: test_arm.py:108
def palmP3dInput(self, p3d)
Definition: test_arm.py:153
def printP3D(self, p3d)
Definition: test_arm.py:92
def __init__(self, args)
Definition: test_arm.py:82
def test_arm(self)
Definition: test_arm.py:198


pr2_gazebo
Author(s): John Hsu
autogenerated on Fri May 3 2019 02:24:27