hironx_ros_bridge: test_hironx_ros_bridge

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2014, JSK Lab, University of Tokyo
00007 # Copyright (c) 2014, TORK
00008 # All rights reserved.
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions
00012 # are met:
00013 #
00014 #  * Redistributions of source code must retain the above copyright
00015 #    notice, this list of conditions and the following disclaimer.
00016 #  * Redistributions in binary form must reproduce the above
00017 #    copyright notice, this list of conditions and the following
00018 #    disclaimer in the documentation and/or other materials provided
00019 #    with the distribution.
00020 #  * Neither the name of JSK Lab, University of Tokyo, TORK, nor the
00021 #    names of its contributors may be used to endorse or promote products
00022 #    derived from this software without specific prior written permission.
00023 #
00024 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 
00037 import numpy
00038 import rospy
00039 from tf.transformations import quaternion_matrix, euler_from_matrix
00040 import time
00041 
00042 from hironx_ros_bridge.testutil.test_rosbridge import TestHiroROSBridge
00043 
00044 PKG = 'hironx_ros_bridge'
00045 
00046 
00047 class TestHiroROSBridgePose(TestHiroROSBridge):
00048 
00049     def test_LArmIK(self):
00050         #   /WAIST /LARM_JOINT5_Link
00051         # - Translation: [0.325, 0.182, 0.074]
00052         # - Rotation: in Quaternion [-0.000, -0.707, 0.000, 0.707]
00053         #             in RPY [-1.694, -1.571, 1.693]
00054         for torso_angle in ([0, -10, 10]):
00055             torso_goal = self.goal_Torso()
00056             torso_goal = self.setup_Positions(torso_goal, [[torso_angle]])
00057             self.torso.send_goal_and_wait(torso_goal)
00058             for p in [[ 0.325, 0.182, 0.074], # initial
00059                       [ 0.3, -0.2, 0.074], [ 0.3, -0.1, 0.074], 
00060                       #[ 0.3,  0.0, 0.074], [ 0.3,  0.1, 0.074], 
00061                       [ 0.3,  0.2, 0.074], [ 0.3,  0.3, 0.074],
00062                       [ 0.4, -0.1, 0.074], [ 0.4, -0.0, 0.074],
00063                       #[ 0.4,  0.1, 0.074], [ 0.4,  0.2, 0.074], 
00064                       [ 0.4,  0.3, 0.074], [ 0.4,  0.4, 0.074],
00065                       ] :
00066 
00067                 print "solve ik at p = ", p
00068                 ret = self.set_target_pose('LARM', p, [-1.694,-1.571, 1.693], 1.0)
00069                 self.assertTrue(ret.operation_return, "ik failed")
00070                 ret = self.wait_interpolation_of_group('LARM')
00071                 self.assertTrue(ret.operation_return, "wait interpolation failed")
00072 
00073                 rospy.sleep(1)
00074                 now = rospy.Time.now()
00075                 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00076                 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00077                 numpy.testing.assert_array_almost_equal(trans, p, decimal=1)
00078                 print "current position   p = ", trans
00079                 print "                 rot = ", rot
00080                 print "                     = ", quaternion_matrix(rot)[0:3,0:3]
00081                 # this fails?
00082                 #numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00083                 #                                        numpy.array([[ 0,  0, -1],
00084                 #                                                     [ 0,  1,  0],
00085                 #                                                     [ 1,  0,  0]]),
00086                 #                                        decimal=3)
00087 
00088 
00089     def test_LArm(self):
00090         goal = self.goal_LArm()
00091         goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00092                                            [  25,-139,-157,  45,   0,   0],
00093                                            [ 0.618, -0.157,-100.0,-15.212, 9.501, -3.188]])
00094         self.larm.send_goal_and_wait(goal)
00095 
00096         rospy.sleep(1)
00097         now = rospy.Time.now()
00098         self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00099         (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00100         numpy.testing.assert_array_almost_equal(trans, [0.325493, 0.18236, 0.0744674], decimal=3)
00101         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00102                                                 numpy.array([[ 0, 0,-1],
00103                                                              [ 0, 1, 0],
00104                                                              [ 1, 0, 0]]), decimal=2)
00105 
00106     def test_RArm(self):
00107         goal = self.goal_RArm()
00108         goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00109                                            [  25,-139,-157,  45,   0,   0],
00110                                            [-0.618, -0.157,-100.0,15.212, 9.501, 3.188]])
00111         self.rarm.send_goal_and_wait(goal)
00112 
00113         rospy.sleep(1)
00114         now = rospy.Time.now()
00115         self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00116         (trans, rot) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00117         numpy.testing.assert_array_almost_equal(trans, [0.325493,-0.18236, 0.0744674], decimal=3)
00118         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00119                                                 numpy.array([[ 0, 0,-1],
00120                                                              [ 0, 1, 0],
00121                                                              [ 1, 0, 0]]), decimal=2)
00122 
00123     def test_Torso(self):
00124         goal = self.goal_Torso()
00125         goal = self.setup_Positions(goal, [[   0.0],
00126                                            [-162.949],
00127                                            [ 162.949]])
00128         self.torso.send_goal_and_wait(goal)
00129 
00130         rospy.sleep(1)
00131         now = rospy.Time.now()
00132         self.listener.waitForTransform("WAIST", "CHEST_JOINT0_Link", now, rospy.Duration(1.0))
00133         (trans, rot) = self.listener.lookupTransform("WAIST", "CHEST_JOINT0_Link", now)
00134         numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.0], decimal=3)
00135         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00136                                                 numpy.array([[-0.956044, -0.293223, 0.0],
00137                                                              [ 0.293223, -0.956044, 0.0],
00138                                                              [ 0.0,       0.0,      1.0]]), decimal=3)
00139 
00140     def test_Head(self):
00141         goal = self.goal_Head()
00142         goal = self.setup_Positions(goal, [[  0.0,  0.0],
00143                                            [ 70.0, 70.0],
00144                                            [-70.0,-20.0]])
00145         self.head.send_goal_and_wait(goal)
00146 
00147         rospy.sleep(1)
00148         now = rospy.Time.now()
00149         self.listener.waitForTransform("WAIST", "HEAD_JOINT1_Link", now, rospy.Duration(1.0))
00150         (trans, rot) = self.listener.lookupTransform("WAIST", "HEAD_JOINT1_Link", now)
00151         numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.5695], decimal=3)
00152         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00153                                                 numpy.array([[0.321394, 0.939693, -0.116978],
00154                                                              [-0.883022, 0.34202, 0.321394],
00155                                                              [0.34202, 0.0, 0.939693]]), decimal=3)
00156 
00157 if __name__ == '__main__':
00158     import rostest
00159     rostest.rosrun(PKG, 'test_hronx_ros_bridge_pose', TestHiroROSBridgePose)
00160 
00161 
00162