hironx_moveit_config: test_hironx

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2015, Tokyo Opensource Robotics Kyokai Association
00006 # (TORK) All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of TORK. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # Author: Isaac I.Y. Saito
00036 
00037 # This test script needs improved so that it becomes call-able from ROS test
00038 # structure.
00039 
00040 import numpy
00041 import unittest
00042 
00043 from geometry_msgs.msg import Pose, PoseStamped
00044 from moveit_commander import MoveGroupCommander
00045 import rospy
00046 import tf
00047 
00048 from hironx_ros_bridge.ros_client import ROS_Client
00049 
00050 _PKG = 'hironx_ros_bridge'
00051 _SEC_WAIT_BETWEEN_TESTCASES = 3
00052 
00053 
00054 class TestHironxMoveit(unittest.TestCase):
00055 
00056     def __init__(self, *args, **kwargs):
00057         super(TestHironxMoveit, self).__init__(*args, **kwargs)
00058 
00059     @classmethod
00060     def setUpClass(self):
00061 
00062         self._ros = ROS_Client()
00063 
00064         self._botharms_joints = ['LARM_JOINT0', 'LARM_JOINT1',
00065                                  'LARM_JOINT2', 'LARM_JOINT3',
00066                                  'LARM_JOINT4', 'LARM_JOINT5',
00067                                  'RARM_JOINT0', 'RARM_JOINT1',
00068                                  'RARM_JOINT2', 'RARM_JOINT3',
00069                                  'RARM_JOINT4', 'RARM_JOINT5']
00070 
00071         self._ros.MG_RARM_current_pose = self._ros.MG_RARM.get_current_pose().pose
00072         self._ros.MG_LARM_current_pose = self._ros.MG_LARM.get_current_pose().pose
00073         # For botharms get_current_pose ends with no eef error.
00074 
00075         self.init_rtm_jointvals = [0.010471975511965976, 0.0, -1.7453292519943295, -0.26529004630313807, 0.16406094968746698, -0.05585053606381855,
00076                                    -0.010471975511965976, 0.0, -1.7453292519943295, 0.26529004630313807, 0.16406094968746698, 0.05585053606381855]
00077 
00078         self.init_rtm_jointvals_factory = [-1.3877787807814457e-17, 1.0842021724855044e-19, -2.2689280275926285, -4.440892098500626e-16, -2.220446049250313e-16, 0.0,
00079                                            0.0, 1.0842021724855044e-19, -2.2689280275926285, 2.220446049250313e-16, -1.1102230246251565e-16, 5.551115123125783e-17]
00080 
00081         self.offpose_jointvals = [0.0, 0.0, 0.0,
00082                                   -0.4363323129985819, -2.4260076602721163, -2.7401669256310983, -0.7853981633974487, 0.0, 0.0,
00083                                   0.4363323129985819, -2.4260076602721163, -2.7401669256310983, 0.7853981633974487, 0.0, 0.0]
00084 
00085         # These represent a pose as in the image https://goo.gl/hYa15h
00086         self.banzai_pose_larm_goal = [-0.0280391167993, 0.558512828409, 0.584801820449,
00087                                       0.468552399035, -0.546097642377, -0.449475560632, 0.529346516701]
00088         self.banzai_pose_rarm_goal = [0.0765219167208, -0.527210000725, 0.638387081642,
00089                                       -0.185009037721, -0.683111796219, 0.184872589841, 0.681873929223]
00090         self.banzai_jointvals_goal = [1.3591412928962834, -1.5269810342586994, -1.5263864987632225, -0.212938998306429,
00091                                       -0.19093239258017988, -1.5171864827145887, -0.7066724299606867, -1.9314110634425135,
00092                                       -1.4268663042616962, 1.0613942164863952, 0.9037643195141568, 1.835342100423069]
00093 
00094     def _set_target_random(self):
00095         '''
00096         @type self: moveit_commander.MoveGroupCommander
00097         @param self: In this particular test script, the argument "self" is
00098                      either 'rarm' or 'larm'.
00099         '''
00100         global current, current2, target
00101         current = self.get_current_pose()
00102         print "*current*", current
00103         target = self.get_random_pose()
00104         print "*target*", target
00105         self.set_pose_target(target)
00106         self.go()
00107         current2 = self.get_current_pose()
00108         print "*current2*", current2
00109 
00110     # Associate a method to MoveGroupCommander class. This enables users to use
00111     # the method on interpreter.
00112     # Although not sure if this is the smartest Python code, this works fine from
00113     # Python interpreter.
00114     # #MoveGroupCommander._set_target_random = _set_target_random
00115 
00116     # ****** Usage ******
00117     #
00118     # See wiki tutorial
00119     #  https://code.google.com/p/rtm-ros-robotics/wiki/hironx_ros_bridge_en#Manipulate_Hiro_with_Moveit_via_Python
00120 
00121     def test_set_pose_target_rpy(self):
00122         # #rpy ver
00123         target = [0.2035, -0.5399, 0.0709, 0, -1.6, 0]
00124         self._ros.MG_RARM.set_pose_target(target)
00125         self._ros.MG_RARM.plan()
00126         self.assertTrue(self._ros.MG_RARM.go())
00127 
00128     def test_set_pose_target_quarternion(self):
00129         target = [0.2035, -0.5399, 0.0709, 0.000427, 0.000317, -0.000384, 0.999999]
00130         self._ros.MG_RARM.set_pose_target(target)
00131         self._ros.MG_RARM.plan()
00132         self.assertTrue(self._ros.MG_RARM.go())
00133 
00134     def test_botharms_compare_joints(self):
00135         '''Comparing joint names.'''
00136         self.assertItemsEqual(self._ros.MG_BOTHARMS.get_joints(), self._botharms_joints)
00137 
00138     def test_botharms_get_joint_values(self):
00139         '''
00140         "both_arm" move group can't set pose target for now (July, 2015) due to
00141         missing eef in the moveit config. Here checking if
00142         MoveGroup.get_joint_values is working.
00143         '''
00144 
00145         self._ros.MG_LARM.set_pose_target(self.banzai_pose_larm_goal)
00146         self._ros.MG_LARM.plan()
00147         self._ros.MG_LARM.go()
00148         self._ros.MG_RARM.set_pose_target(self.banzai_pose_rarm_goal)
00149         self._ros.MG_RARM.plan()
00150         self._ros.MG_RARM.go()
00151         # Comparing to the 3rd degree seems too aggressive; example output:
00152         #   x: array([ 1.35906843, -1.52695742, -1.52658066, -0.21309358,
00153         #              -0.19092542, -1.51707957, -0.70651268, -1.93170852,
00154         #              -1.42660669,  1.0629058, 0.90412021,  1.83650476])
00155         #   y: array([ 1.35914129, -1.52698103, -1.5263865 , -0.212939,
00156         #              -0.19093239, -1.51718648, -0.70667243, -1.93141106,
00157         #              -1.4268663 ,  1.06139422, 0.90376432,  1.8353421 ])
00158         numpy.testing.assert_almost_equal(self._ros.MG_BOTHARMS.get_current_joint_values(),
00159                                           self.banzai_jointvals_goal, 2)
00160 
00161     def test_botharms_set_named_target(self):
00162         '''
00163         Test if moveit_commander.set_named_target brings the arms to
00164         the init_rtm pose defined in HiroNx.srdf.
00165         '''
00166         # Move the arms to non init pose.
00167         self._ros.MG_LARM.set_pose_target(self.banzai_pose_larm_goal)
00168         self._ros.MG_LARM.plan()
00169         self._ros.MG_LARM.go()
00170         self._ros.MG_RARM.set_pose_target(self.banzai_pose_rarm_goal)
00171         self._ros.MG_RARM.plan()
00172         self._ros.MG_RARM.go()
00173 
00174         self._ros.MG_BOTHARMS.set_named_target('init_rtm')
00175         self._ros.MG_BOTHARMS.plan()
00176         self._ros.MG_BOTHARMS.go()
00177 
00178         # Raises AssertException when the assertion fails, which
00179         # automatically be flagged false by unittest framework.
00180         # http://stackoverflow.com/a/4319836/577001
00181         numpy.testing.assert_almost_equal(self._ros.MG_BOTHARMS.get_current_joint_values(),
00182                                           self.init_rtm_jointvals, 3)
00183 
00184 #    def test_simple_unittest(self):
00185 #        self.assertEqual(1, 1)
00186 
00187     def test_rosclient_robotcommander(self):
00188         '''
00189         Starting from https://github.com/start-jsk/rtmros_hironx/pull/422,
00190         ROS_Client.py depends on moveit_commander.RobotCommander class.
00191         This case tests the integration of ROS_Client.py and RobotCommander.
00192 
00193         Developers need to avoid testing moveit_commander.RobotCommander itself
00194         -- that needs to be done upstream.
00195         '''
00196         # If the list of movegroups are not none, that can mean
00197         # RobotCommander is working as expected.
00198         groupnames = self._ros.get_group_names()
00199         self.assertIsNotNone(groupnames)
00200 
00201     def test_rosclient_goInitial(self):
00202         self._ros.goInitial()
00203         numpy.testing.assert_almost_equal(self._ros.MG_BOTHARMS.get_current_joint_values(),
00204                                           self.init_rtm_jointvals, 3)
00205         self._ros.goInitial(init_pose_type=1)
00206         numpy.testing.assert_almost_equal(self._ros.MG_BOTHARMS.get_current_joint_values(),
00207                                           self.init_rtm_jointvals_factory, 3)
00208 
00209     def test_rosclient_go_offpose(self):
00210         self._ros.go_offpose()
00211         numpy.testing.assert_almost_equal(self._ros.MG_UPPERBODY.get_current_joint_values(),
00212                                           self.offpose_jointvals, 3)
00213 
00214     def test_geometry_link_r4_r5(self):
00215         ''' 
00216             Trying to capture issues like https://github.com/tork-a/rtmros_nextage/issues/244
00217         '''
00218         TRANS_RARM4_RARM5 = [-0.047, 0.000, -0.090]  # Expected value.
00219 
00220         tflistener = tf.TransformListener()
00221         (trans, rot) = tflistener.lookupTransform("/RARM_JOINT5_Link", "/RARM_JOINT4_Link", rospy.Time(0))
00222         [self.assertAlmostEqual(v_trans, v_expected, 3) for v_trans, v_expected in zip(trans, TRANS_RARM4_RARM5)]
00223 
00224 
00225 if __name__ == '__main__':
00226     import rostest
00227     rostest.rosrun(_PKG, 'test_hironx_moveit_run', TestHironxMoveit)