acceptancetest_hironx.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
00006 # 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
00019 #    names of its contributors may be used to endorse or promote products
00020 #    derived 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 import time, rospy
00036 
00037 from hironx_ros_bridge.constant import Constant
00038 from hironx_ros_bridge.hironx_client import HIRONX
00039 from hironx_ros_bridge.ros_client import ROS_Client
00040 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
00041 from hironx_ros_bridge.testutil.acceptancetest_ros import AcceptanceTestROS
00042 from hironx_ros_bridge.testutil.acceptancetest_rtm import AcceptanceTestRTM
00043 
00044 
00045 class AcceptanceTest_Hiro():
00046     '''
00047     This class holds methods that can be used for verification of the robot
00048     Kawada Industries' dual-arm robot Hiro (and the same model of other robots
00049     e.g. NEXTAGE OPEN).
00050 
00051     This test class is:
00052     - Intended to be run as nosetests, ie. roscore isn't available by itself.
00053     - Almost all the testcases don't run without an access to a robot running.
00054 
00055     Above said, combined with a rostest that launches roscore and robot (either
00056     simulation or real) hopefully these testcases can be run, both in batch
00057     manner by rostest and in nosetest manner.
00058 
00059     Prefix for methods 'at' means 'acceptance test'.
00060 
00061     All time arguments are second.
00062     '''
00063 
00064     _POSITIONS_LARM_DEG_UP = [-4.697, -2.012, -117.108, -17.180, 29.146, -3.739]
00065     _POSITIONS_LARM_DEG_DOWN = [6.196, -5.311, -73.086, -15.287, -12.906, -2.957]
00066     _POSITIONS_RARM_DEG_DOWN = [-4.949, -3.372, -80.050, 15.067, -7.734, 3.086]
00067     _POSITIONS_LARM_DEG_UP_SYNC = [-4.695, -2.009, -117.103, -17.178, 29.138, -3.738]
00068     _POSITIONS_RARM_DEG_UP_SYNC = [4.695, -2.009, -117.103, 17.178, 29.138, 3.738]
00069     _ROTATION_ANGLES_HEAD_1 = [[0.1, 0.0], [50.0, 10.0]]
00070     _ROTATION_ANGLES_HEAD_2 = [[50, 10], [-50, -10], [0, 0]]
00071     _POSITIONS_TORSO_DEG = [[130.0], [-130.0]]
00072     _R_Z_SMALLINCREMENT = 0.0001
00073     # Workspace; X near, Y far
00074     _POS_L_X_NEAR_Y_FAR = [0.326, 0.474, 1.038]
00075     _RPY_L_X_NEAR_Y_FAR = (-3.075, -1.569, 3.074)
00076     _POS_R_X_NEAR_Y_FAR = [0.326, -0.472, 1.048]
00077     _RPY_R_X_NEAR_Y_FAR = (3.073, -1.569, -3.072)
00078     # Workspace; X far, Y far
00079     _POS_L_X_FAR_Y_FAR = [0.47548142379781055, 0.17430276793604782, 1.0376878025614884]
00080     _RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046, 3.0757659493049574)
00081     _POS_R_X_FAR_Y_FAR = [0.4755337947019357, -0.17242322190721648, 1.0476395479774052]
00082     _RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248, -3.071395243174742)
00083 
00084     _TASK_DURATION_DEFAULT = 5.0
00085     _DURATION_EACH_SMALLINCREMENT = 0.1
00086     _TASK_DURATION_TORSO = 4.0
00087     _TASK_DURATION_HEAD = 2.5
00088 
00089     _DURATON_TOTAL_SMALLINCREMENT = 30  # Second.
00090 
00091     def __init__(self, rtm_robotname='HiroNX(Robot)0', url=''):
00092         '''
00093         Initiate both ROS and RTM robot clients, keep them public so that they
00094         are callable from script. e.g. On ipython,
00095 
00096                 In [1]: acceptance.ros.go_init()
00097                 In [2]: acceptance.rtm.goOffPose()
00098         '''
00099         self._rtm_robotname = rtm_robotname
00100         self._rtm_url = url
00101 
00102         # Init RTM and ROS client.
00103         self.ros = ROS_Client()
00104         self._acceptance_ros_client = AcceptanceTestROS(self.ros)
00105         if rospy.has_param('/gazebo'):
00106             print("\033[32m[INFO] Assming Gazebo Siulator, so do not connect to CORBA systmes\033[0m")
00107             print("\033[32m[INFO] run 'acceptance.run_tests_ros()' for ROS interface test\033[0m")
00108         else:
00109             self.rtm = HIRONX()
00110             self.rtm.init(robotname=self._rtm_robotname, url=self._rtm_url)
00111             self._acceptance_rtm_client = AcceptanceTestRTM(self.rtm)
00112             print("\033[32m[INFO] run 'acceptance.run_tests_rtm()' for RTM interface test\033[0m")
00113 
00114     def _wait_input(self, msg, do_wait_input=False):
00115         '''
00116         @type msg: str
00117         @param msg: To be printed on prompt.
00118         @param do_wait_input: If True python commandline waits for any input
00119                               to proceed.
00120         '''
00121         if msg:
00122             msg = '\n' + msg + '= '
00123         if do_wait_input:
00124             try:
00125                 input('Waiting for any key. **Do NOT** hit enter multiple ' +
00126                       'times. ' + msg)
00127             except NameError:
00128                 # On python < 3, `input` can cause errors like following with
00129                 # any input string. So here just catch-release it.
00130                 # e.g.
00131                 #   NameError: name 'next' is not defined
00132                 pass
00133 
00134     def run_tests_rtm(self, do_wait_input=False):
00135         '''
00136         @param do_wait_input: If True, the user will be asked to hit any key
00137                               before each task to proceed.
00138         '''
00139         self._run_tests(self._acceptance_rtm_client, do_wait_input)
00140 
00141     def run_tests_ros(self, do_wait_input=False):
00142         '''
00143         Run by ROS exactly the same actions that run_tests_rtm performs.
00144 
00145         @param do_wait_input: If True, the user will be asked to hit any key
00146                               before each task to proceed.
00147         '''
00148         self._run_tests(self._acceptance_ros_client, do_wait_input)
00149 
00150     def _run_tests(self, test_client, do_wait_input=False):
00151         '''
00152         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
00153         '''
00154 
00155         _task_msgs = ['TASK-1-1. Move each arm separately to the given pose by passing joint space.',
00156                       'TASK-1-2. Move each arm separately to the given pose by specifying a pose.',
00157                       'TASK-2. Move both arms at the same time using relative distance and angle from the current pose.',
00158                       'TASK-3. Move arm with very small increment (0.1mm/sec).\n\tRight hand 3 cm upward over 30 seconds.',
00159                       'In the beginning you\'ll see the displacement of the previous task.' +
00160                        '\nTASK-4. Move head using Joint angles in degree.',
00161                       'TASK-5. Rotate torso to the left and right 130 degree.',
00162                       'TASK-6. Overwrite ongoing action.' +
00163                       '\n\t6-1. While rotating torso toward left, it gets canceled and rotate toward right.' +
00164                       '\n\t6-2. While lifting left hand, right hand also tries to reach the same height that gets cancelled so that it stays lower than the left hand.',
00165                       'TASK-7. Show the capable workspace on front side of the robot.']
00166 
00167         _msg_type_client = None
00168         if isinstance(test_client, AcceptanceTestROS):
00169             _msg_type_client = '(ROS) '
00170         elif isinstance(test_client, AcceptanceTestRTM):
00171             _msg_type_client = '(RTM) '
00172 
00173         test_client.go_initpos()
00174 
00175         msg_task = _task_msgs[0]
00176         msg = _msg_type_client + msg_task
00177         self._wait_input(msg, do_wait_input)
00178         self._move_armbyarm_jointangles(test_client)
00179 
00180         msg_task = _task_msgs[1]
00181         msg = _msg_type_client + msg_task
00182         self._wait_input(msg, do_wait_input)
00183         self._move_armbyarm_pose(test_client)
00184 
00185         msg_task = _task_msgs[2]
00186         msg = _msg_type_client + msg_task
00187         self._wait_input(msg, do_wait_input)
00188         self._movearms_together(test_client)
00189 
00190         msg_task = _task_msgs[3]
00191         msg = _msg_type_client + msg_task
00192         self._wait_input(msg, do_wait_input)
00193         self._set_pose_relative(test_client)
00194 
00195         msg_task = _task_msgs[4]
00196         msg = _msg_type_client + msg_task
00197         self._wait_input(msg, do_wait_input)
00198         self._move_head(test_client)
00199 
00200         msg_task = _task_msgs[5]
00201         msg = _msg_type_client + msg_task
00202         self._wait_input(msg, do_wait_input)
00203         self._move_torso(test_client)
00204 
00205         msg_task = _task_msgs[6]
00206         msg = _msg_type_client + msg_task
00207         self._wait_input(msg, do_wait_input)
00208         self._overwrite_torso(test_client)
00209         self._overwrite_arm(test_client)
00210 
00211         msg_task = _task_msgs[7]
00212         msg = _msg_type_client + msg_task
00213         self._wait_input(msg, do_wait_input)
00214         self._show_workspace(test_client)
00215 
00216     def _move_armbyarm_jointangles(self, test_client):
00217         '''
00218         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest  
00219         '''
00220         test_client.go_initpos()
00221         arm = Constant.GRNAME_LEFT_ARM
00222         test_client.set_joint_angles(arm, self._POSITIONS_LARM_DEG_UP,
00223                                      'Task1 {}'.format(arm))
00224 
00225         arm = Constant.GRNAME_RIGHT_ARM
00226         test_client.set_joint_angles(arm, self._POSITIONS_RARM_DEG_DOWN,
00227                                      'Task1 {}'.format(arm))
00228         time.sleep(2.0)
00229 
00230     def _move_armbyarm_pose(self, test_client):
00231         print('** _move_armbyarm_pose isn\'t yet implemented')
00232         pass
00233 
00234     def _movearms_together(self, test_client):
00235         '''
00236         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest  
00237         '''
00238         test_client.go_initpos()
00239         arm = Constant.GRNAME_LEFT_ARM
00240         test_client.set_joint_angles(
00241                  arm, self._POSITIONS_LARM_DEG_UP_SYNC, '{}'.format(arm),
00242                  self._TASK_DURATION_DEFAULT, False)
00243                 #'task2; Under current implementation, left arm ' +
00244                 #'always moves first, followed immediately by right arm')
00245         arm = Constant.GRNAME_RIGHT_ARM
00246         test_client.set_joint_angles(
00247                     arm, self._POSITIONS_RARM_DEG_DOWN, '{}'.format(arm),
00248                     self._TASK_DURATION_DEFAULT, False)
00249         time.sleep(2.0)
00250 
00251     def _set_pose_relative(self, test_client):
00252         test_client.go_initpos()
00253 
00254         delta = self._R_Z_SMALLINCREMENT
00255         t = self._DURATION_EACH_SMALLINCREMENT
00256         t_total_sec = self._DURATON_TOTAL_SMALLINCREMENT
00257         total_increment = int(t_total_sec / t)
00258         msg_1 = ('Right arm is moving upward with' +
00259                  '{}mm increment per {}s'.format(delta, t))
00260         msg_2 = ' (Total {}cm over {}s).'.format(delta * total_increment, t_total_sec)
00261         print(msg_1 + msg_2)
00262         for i in range(total_increment):
00263             msg_eachloop = '{}th loop;'.format(i)
00264             test_client.set_pose_relative(
00265                      Constant.GRNAME_RIGHT_ARM, dz=delta,
00266                      msg_tasktitle=msg_eachloop, task_duration=t, do_wait=True)
00267 
00268     def _move_head(self, test_client):
00269         test_client.go_initpos()
00270 
00271         for positions in self._ROTATION_ANGLES_HEAD_1:
00272             test_client.set_joint_angles(
00273                                Constant.GRNAME_HEAD,
00274                                positions, '(1);', self._TASK_DURATION_HEAD)
00275 
00276         for positions in self._ROTATION_ANGLES_HEAD_2:
00277             test_client.set_joint_angles(
00278                                      Constant.GRNAME_HEAD, positions,
00279                                      '(2);', self._TASK_DURATION_HEAD)
00280 
00281     def _move_torso(self, test_client):
00282         test_client.go_initpos()
00283         for positions in self._POSITIONS_TORSO_DEG:
00284             test_client.set_joint_angles(Constant.GRNAME_TORSO,
00285                                          positions, '')
00286 
00287     def _overwrite_torso(self, test_client):
00288         test_client.go_initpos()
00289         test_client.set_joint_angles(
00290                         Constant.GRNAME_TORSO, self._POSITIONS_TORSO_DEG[0],
00291                         '(1)', self._TASK_DURATION_TORSO, False)
00292         time.sleep(2.0)
00293         test_client.set_joint_angles(
00294                           Constant.GRNAME_TORSO, self._POSITIONS_TORSO_DEG[1],
00295                           '(2)', self._TASK_DURATION_TORSO, True)
00296         time.sleep(2.0)
00297 
00298     def _overwrite_arm(self, test_client):
00299         test_client.go_initpos()
00300         test_client.set_joint_angles(
00301                     Constant.GRNAME_LEFT_ARM, self._POSITIONS_LARM_DEG_UP_SYNC,
00302                     '(1)', self._TASK_DURATION_DEFAULT, False)
00303         test_client.set_joint_angles(
00304                    Constant.GRNAME_RIGHT_ARM, self._POSITIONS_RARM_DEG_UP_SYNC,
00305                    '(2) begins. Overwrite previous arm command.' +
00306                    '\n\tIn the beginning both arm starts to move to the' +
00307                    '\n\tsame height, but to the left arm interrupting' +
00308                    '\ncommand is sent and it goes downward.',
00309                    self._TASK_DURATION_DEFAULT, False)
00310         time.sleep(2.0)
00311         test_client.set_joint_angles(
00312                       Constant.GRNAME_LEFT_ARM, self._POSITIONS_LARM_DEG_DOWN,
00313                       '(3)', self._TASK_DURATION_DEFAULT, False)
00314 
00315     def _show_workspace(self, test_client):
00316         test_client.go_initpos()
00317         msg = '; '
00318 
00319         larm = Constant.GRNAME_LEFT_ARM
00320         rarm = Constant.GRNAME_RIGHT_ARM
00321         # X near, Y far.
00322         test_client.set_pose(
00323                    larm, self._POS_L_X_NEAR_Y_FAR, self._RPY_L_X_NEAR_Y_FAR,
00324                    msg + '{}'.format(larm), self._TASK_DURATION_DEFAULT, False)
00325         test_client.set_pose(
00326                    rarm, self._POS_R_X_NEAR_Y_FAR, self._RPY_R_X_NEAR_Y_FAR,
00327                    msg + '{}'.format(rarm), self._TASK_DURATION_DEFAULT, True)
00328 
00329         # X far, Y far.
00330         test_client.set_pose(
00331                    larm, self._POS_L_X_FAR_Y_FAR, self._RPY_L_X_FAR_Y_FAR,
00332                    msg + '{}'.format(larm), self._TASK_DURATION_DEFAULT, False)
00333         test_client.set_pose(
00334                     rarm, self._POS_R_X_FAR_Y_FAR, self._RPY_R_X_FAR_Y_FAR,
00335                     msg + '{}'.format(rarm), self._TASK_DURATION_DEFAULT, True)
00336 
00337 import argparse
00338 
00339 from hrpsys import rtm
00340 
00341 if __name__ == '__main__':
00342     parser = argparse.ArgumentParser(description='hiro command line interpreters')
00343     parser.add_argument('--host', help='corba name server hostname')
00344     parser.add_argument('--port', help='corba name server port number')
00345     parser.add_argument('--modelfile', help='robot model file nmae')
00346     parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
00347     args, unknown = parser.parse_known_args()
00348 
00349     if args.host:
00350         rtm.nshost = args.host
00351     if args.port:
00352         rtm.nsport = args.port
00353     if not args.robot:
00354         args.robot = 'RobotHardware0' if args.host else 'HiroNX(Robot)0'
00355     if not args.modelfile:
00356         args.modelfile = ''
00357 
00358     # support old style format
00359     if len(unknown) >= 2:
00360         args.robot = unknown[0]
00361         args.modelfile = unknown[1]
00362     acceptance = AcceptanceTest_Hiro()


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37