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
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         self.rtm = HIRONX()
00106         self.rtm.init(robotname=self._rtm_robotname, url=self._rtm_url)
00107         self._acceptance_rtm_client = AcceptanceTestRTM(self.rtm)
00108 
00109     def _wait_input(self, msg, do_wait_input=False):
00110         '''
00111         @type msg: str
00112         @param msg: To be printed on prompt.
00113         @param do_wait_input: If True python commandline waits for any input
00114                               to proceed.
00115         '''
00116         if msg:
00117             msg = '\n' + msg + '= '
00118         if do_wait_input:
00119             try:
00120                 input('Waiting for any key. **Do NOT** hit enter multiple ' +
00121                       'times. ' + msg)
00122             except NameError:
00123                 # On python < 3, `input` can cause errors like following with
00124                 # any input string. So here just catch-release it.
00125                 # e.g.
00126                 #   NameError: name 'next' is not defined
00127                 pass
00128 
00129     def run_tests_rtm(self, do_wait_input=False):
00130         '''
00131         @param do_wait_input: If True, the user will be asked to hit any key
00132                               before each task to proceed.
00133         '''
00134         self._run_tests(self._acceptance_rtm_client, do_wait_input)
00135 
00136     def run_tests_ros(self, do_wait_input=False):
00137         '''
00138         Run by ROS exactly the same actions that run_tests_rtm performs.
00139 
00140         @param do_wait_input: If True, the user will be asked to hit any key
00141                               before each task to proceed.
00142         '''
00143         self._run_tests(self._acceptance_ros_client, do_wait_input)
00144 
00145     def _run_tests(self, test_client, do_wait_input=False):
00146         '''
00147         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
00148         '''
00149 
00150         _task_msgs = ['TASK-1-1. Move each arm separately to the given pose by passing joint space.',
00151                       'TASK-1-2. Move each arm separately to the given pose by specifying a pose.',
00152                       'TASK-2. Move both arms at the same time using relative distance and angle from the current pose.',
00153                       'TASK-3. Move arm with very small increment (0.1mm/sec).\n\tRight hand 3 cm upward over 30 seconds.',
00154                       'In the beginning you\'ll see the displacement of the previous task.' +
00155                        '\nTASK-4. Move head using Joint angles in degree.',
00156                       'TASK-5. Rotate torso to the left and right 130 degree.',
00157                       'TASK-6. Overwrite ongoing action.' +
00158                       '\n\t6-1. While rotating torso toward left, it gets canceled and rotate toward right.' +
00159                       '\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.',
00160                       'TASK-7. Show the capable workspace on front side of the robot.']
00161 
00162         _msg_type_client = None
00163         if isinstance(test_client, AcceptanceTestROS):
00164             _msg_type_client = '(ROS) '
00165         elif isinstance(test_client, AcceptanceTestRTM):
00166             _msg_type_client = '(RTM) '
00167 
00168         test_client.go_initpos()
00169 
00170         msg_task = _task_msgs[0]
00171         msg = _msg_type_client + msg_task
00172         self._wait_input(msg, do_wait_input)
00173         self._move_armbyarm_jointangles(test_client)
00174 
00175         msg_task = _task_msgs[1]
00176         msg = _msg_type_client + msg_task
00177         self._wait_input(msg, do_wait_input)
00178         self._move_armbyarm_pose(test_client)
00179 
00180         msg_task = _task_msgs[2]
00181         msg = _msg_type_client + msg_task
00182         self._wait_input(msg, do_wait_input)
00183         self._movearms_together(test_client)
00184 
00185         msg_task = _task_msgs[3]
00186         msg = _msg_type_client + msg_task
00187         self._wait_input(msg, do_wait_input)
00188         self._set_pose_relative(test_client)
00189 
00190         msg_task = _task_msgs[4]
00191         msg = _msg_type_client + msg_task
00192         self._wait_input(msg, do_wait_input)
00193         self._move_head(test_client)
00194 
00195         msg_task = _task_msgs[5]
00196         msg = _msg_type_client + msg_task
00197         self._wait_input(msg, do_wait_input)
00198         self._move_torso(test_client)
00199 
00200         msg_task = _task_msgs[6]
00201         msg = _msg_type_client + msg_task
00202         self._wait_input(msg, do_wait_input)
00203         self._overwrite_torso(test_client)
00204         self._overwrite_arm(test_client)
00205 
00206         msg_task = _task_msgs[7]
00207         msg = _msg_type_client + msg_task
00208         self._wait_input(msg, do_wait_input)
00209         self._show_workspace(test_client)
00210 
00211     def _move_armbyarm_jointangles(self, test_client):
00212         '''
00213         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest  
00214         '''
00215         test_client.go_initpos()
00216         arm = Constant.GRNAME_LEFT_ARM
00217         test_client.set_joint_angles(arm, self._POSITIONS_LARM_DEG_UP,
00218                                      'Task1 {}'.format(arm))
00219 
00220         arm = Constant.GRNAME_RIGHT_ARM
00221         test_client.set_joint_angles(arm, self._POSITIONS_RARM_DEG_DOWN,
00222                                      'Task1 {}'.format(arm))
00223         time.sleep(2.0)
00224 
00225     def _move_armbyarm_pose(self, test_client):
00226         print('** _move_armbyarm_pose isn\'t yet implemented')
00227         pass
00228 
00229     def _movearms_together(self, test_client):
00230         '''
00231         @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest  
00232         '''
00233         test_client.go_initpos()
00234         arm = Constant.GRNAME_LEFT_ARM
00235         test_client.set_joint_angles(
00236                  arm, self._POSITIONS_LARM_DEG_UP_SYNC, '{}'.format(arm),
00237                  self._TASK_DURATION_DEFAULT, False)
00238                 #'task2; Under current implementation, left arm ' +
00239                 #'always moves first, followed immediately by right arm')
00240         arm = Constant.GRNAME_RIGHT_ARM
00241         test_client.set_joint_angles(
00242                     arm, self._POSITIONS_RARM_DEG_DOWN, '{}'.format(arm),
00243                     self._TASK_DURATION_DEFAULT, False)
00244         time.sleep(2.0)
00245 
00246     def _set_pose_relative(self, test_client):
00247         test_client.go_initpos()
00248 
00249         delta = self._R_Z_SMALLINCREMENT
00250         t = self._DURATION_EACH_SMALLINCREMENT
00251         t_total_sec = self._DURATON_TOTAL_SMALLINCREMENT
00252         total_increment = int(t_total_sec / t)
00253         msg_1 = ('Right arm is moving upward with' +
00254                  '{}mm increment per {}s'.format(delta, t))
00255         msg_2 = ' (Total {}cm over {}s).'.format(delta * total_increment, t_total_sec)
00256         print(msg_1 + msg_2)
00257         for i in range(total_increment):
00258             msg_eachloop = '{}th loop;'.format(i)
00259             test_client.set_pose_relative(
00260                      Constant.GRNAME_RIGHT_ARM, dz=delta,
00261                      msg_tasktitle=msg_eachloop, task_duration=t, do_wait=True)
00262 
00263     def _move_head(self, test_client):
00264         test_client.go_initpos()
00265 
00266         for positions in self._ROTATION_ANGLES_HEAD_1:
00267             test_client.set_joint_angles(
00268                                Constant.GRNAME_HEAD,
00269                                positions, '(1);', self._TASK_DURATION_HEAD)
00270 
00271         for positions in self._ROTATION_ANGLES_HEAD_2:
00272             test_client.set_joint_angles(
00273                                      Constant.GRNAME_HEAD, positions,
00274                                      '(2);', self._TASK_DURATION_HEAD)
00275 
00276     def _move_torso(self, test_client):
00277         test_client.go_initpos()
00278         for positions in self._POSITIONS_TORSO_DEG:
00279             test_client.set_joint_angles(Constant.GRNAME_TORSO,
00280                                          positions, '')
00281 
00282     def _overwrite_torso(self, test_client):
00283         test_client.go_initpos()
00284         test_client.set_joint_angles(
00285                         Constant.GRNAME_TORSO, self._POSITIONS_TORSO_DEG[0],
00286                         '(1)', self._TASK_DURATION_TORSO, False)
00287         time.sleep(2.0)
00288         test_client.set_joint_angles(
00289                           Constant.GRNAME_TORSO, self._POSITIONS_TORSO_DEG[1],
00290                           '(2)', self._TASK_DURATION_TORSO, True)
00291         time.sleep(2.0)
00292 
00293     def _overwrite_arm(self, test_client):
00294         test_client.go_initpos()
00295         test_client.set_joint_angles(
00296                     Constant.GRNAME_LEFT_ARM, self._POSITIONS_LARM_DEG_UP_SYNC,
00297                     '(1)', self._TASK_DURATION_DEFAULT, False)
00298         test_client.set_joint_angles(
00299                    Constant.GRNAME_RIGHT_ARM, self._POSITIONS_RARM_DEG_UP_SYNC,
00300                    '(2) begins. Overwrite previous arm command.' +
00301                    '\n\tIn the beginning both arm starts to move to the' +
00302                    '\n\tsame height, but to the left arm interrupting' +
00303                    '\ncommand is sent and it goes downward.',
00304                    self._TASK_DURATION_DEFAULT, False)
00305         time.sleep(2.0)
00306         test_client.set_joint_angles(
00307                       Constant.GRNAME_LEFT_ARM, self._POSITIONS_LARM_DEG_DOWN,
00308                       '(3)', self._TASK_DURATION_DEFAULT, False)
00309 
00310     def _show_workspace(self, test_client):
00311         test_client.go_initpos()
00312         msg = '; '
00313 
00314         larm = Constant.GRNAME_LEFT_ARM
00315         rarm = Constant.GRNAME_RIGHT_ARM
00316         # X near, Y far.
00317         test_client.set_pose(
00318                    larm, self._POS_L_X_NEAR_Y_FAR, self._RPY_L_X_NEAR_Y_FAR,
00319                    msg + '{}'.format(larm), self._TASK_DURATION_DEFAULT, False)
00320         test_client.set_pose(
00321                    rarm, self._POS_R_X_NEAR_Y_FAR, self._RPY_R_X_NEAR_Y_FAR,
00322                    msg + '{}'.format(rarm), self._TASK_DURATION_DEFAULT, True)
00323 
00324         # X far, Y far.
00325         test_client.set_pose(
00326                    larm, self._POS_L_X_FAR_Y_FAR, self._RPY_L_X_FAR_Y_FAR,
00327                    msg + '{}'.format(larm), self._TASK_DURATION_DEFAULT, False)
00328         test_client.set_pose(
00329                     rarm, self._POS_R_X_FAR_Y_FAR, self._RPY_R_X_FAR_Y_FAR,
00330                     msg + '{}'.format(rarm), self._TASK_DURATION_DEFAULT, True)
00331 
00332 import argparse
00333 
00334 from hrpsys import rtm
00335 
00336 if __name__ == '__main__':
00337     parser = argparse.ArgumentParser(description='hiro command line interpreters')
00338     parser.add_argument('--host', help='corba name server hostname')
00339     parser.add_argument('--port', help='corba name server port number')
00340     parser.add_argument('--modelfile', help='robot model file nmae')
00341     parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
00342     args, unknown = parser.parse_known_args()
00343 
00344     if args.host:
00345         rtm.nshost = args.host
00346     if args.port:
00347         rtm.nsport = args.port
00348     if not args.robot:
00349         args.robot = 'RobotHardware0' if args.host else 'HiroNX(Robot)0'
00350     if not args.modelfile:
00351         args.modelfile = ''
00352 
00353     # support old style format
00354     if len(unknown) >= 2:
00355         args.robot = unknown[0]
00356         args.modelfile = unknown[1]
00357     acceptance = AcceptanceTest_Hiro()


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39