00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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
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
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
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
00129
00130
00131
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
00244
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
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
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
00359 if len(unknown) >= 2:
00360 args.robot = unknown[0]
00361 args.modelfile = unknown[1]
00362 acceptance = AcceptanceTest_Hiro()