hironx_ros_bridge: acceptancetest

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.testutil.abst_acceptancetest import AbstAcceptanceTest
00039 
00040 
00041 class AcceptanceTestRTM(AbstAcceptanceTest):
00042 
00043     def __init__(self, robot_client):
00044         '''
00045         @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
00046         '''
00047         self._robotclient = robot_client
00048 
00049     def go_initpos(self):
00050         self._robotclient.goInitial()
00051 
00052     def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
00053                          task_duration=7.0, do_wait=True):
00054         '''
00055         @see: AbstAcceptanceTest.set_joint_angles
00056         '''
00057         print("== RTM; {} ==".format(msg_tasktitle))
00058         self._robotclient.setJointAnglesOfGroup(
00059                          joint_group, joint_angles, task_duration, do_wait)
00060 
00061     def set_pose(self, joint_group, position, rpy, msg_tasktitle,
00062                  task_duration=7.0, do_wait=True, ref_frame_name=None):
00063 
00064         print("== RTM; {} ==".format(msg_tasktitle))
00065         self._robotclient.setTargetPose(joint_group, position, rpy,
00066                                         task_duration, ref_frame_name)
00067         if do_wait:
00068             self._robotclient.waitInterpolationOfGroup(joint_group)
00069 
00070     def set_pose_relative(
00071                         self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
00072                         msg_tasktitle=None, task_duration=7.0, do_wait=True):
00073         if joint_group == Constant.GRNAME_LEFT_ARM:
00074             eef = 'LARM_JOINT5'
00075         elif joint_group == Constant.GRNAME_RIGHT_ARM:
00076             eef = 'RARM_JOINT5'
00077 
00078         print("== RTM; {} ==".format(msg_tasktitle))
00079         self._robotclient.setTargetPoseRelative(
00080                                     joint_group, eef, dx, dy, dz, dr, dp, dw,
00081                                     task_duration, do_wait)
00082 
00083     def _run_tests_hrpsys(self):
00084         '''
00085         @deprecated: This method remains as a reference. This used to function
00086                      when being called directly from ipython commandline and
00087                      now replaced by optimized codes.
00088         '''
00089         _TIME_SETTARGETP_L = 3
00090         _TIME_SETTARGETP_R = 2
00091         _TIME_BW_TESTS = 5
00092 
00093         self.robot.goInitial()
00094 
00095         # === TASK-1 ===
00096         # L arm setTargetPose
00097         _POS_L_INIT = self.robot.getCurrentPosition('LARM_JOINT5')
00098         _POS_L_INIT[2] += 0.8
00099         _RPY_L_INIT = self.robot.getCurrentRPY('LARM_JOINT5')
00100         self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_SETTARGETP_L)
00101         self.robot.waitInterpolationOfGroup('larm')
00102 
00103         # R arm setTargetPose
00104         _POS_R_INIT = self.robot.getCurrentPosition('RARM_JOINT5')
00105         _POS_R_INIT[2] -= 0.07
00106         _RPY_R_INIT = self.robot.getCurrentRPY('RARM_JOINT5')
00107         self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_SETTARGETP_R)
00108         self.robot.waitInterpolationOfGroup('rarm')
00109         time.sleep(_TIME_BW_TESTS)
00110 
00111         # === TASK-2 === 
00112         self.robot.goInitial()
00113         # Both arm setTargetPose
00114         _Z_SETTARGETP_L = 0.08
00115         _Z_SETTARGETP_R = 0.08
00116         self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
00117                                          dz=_Z_SETTARGETP_L,
00118                                          tm=_TIME_SETTARGETP_L, wait=False)
00119         self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
00120                                          dz=_Z_SETTARGETP_R,
00121                                          tm=_TIME_SETTARGETP_R, wait=False)
00122 
00123         # === TASK-3 ===
00124         # Head toward down
00125         _TIME_HEAD = 5
00126         self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=0.1, tm=_TIME_HEAD)
00127         self.robot.waitInterpolationOfGroup('head')
00128         # Head toward up
00129         self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=-0.2, tm=_TIME_HEAD)
00130         self.robot.waitInterpolationOfGroup('head')
00131         # See left by position
00132         self.robot.setJointAnglesOfGroup('head', [50, 10], 2, wait=True)
00133         # See right by position
00134         self.robot.setJointAnglesOfGroup('head', [-50, -10], 2, wait=True)
00135         # Set back face to the starting pose w/o wait. 
00136         self.robot.setJointAnglesOfGroup( 'head', [0, 0], 2, wait=False)
00137 
00138         # === TASK-4 ===
00139         # 0.1mm increment is not working for some reason.
00140         self.robot.goInitial()
00141         # Move by iterating 0.1mm at cartesian space
00142         _TIME_CARTESIAN = 0.1
00143         _INCREMENT_MIN = 0.0001
00144         for i in range(300):
00145             self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
00146                                              dy=_INCREMENT_MIN,
00147                                              tm=_TIME_CARTESIAN)
00148             self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
00149                                              dy=_INCREMENT_MIN,
00150                                              tm=_TIME_CARTESIAN)
00151             print('{}th move'.format(i))
00152 
00153         self.robot.goInitial()
00154         # === TASK-5 ===
00155         # Turn torso
00156         _TORSO_ANGLE = 120
00157         _TIME_TORSO_R = 7
00158         self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R, wait=True)
00159         self.robot.waitInterpolationOfGroup('torso')
00160         self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
00161  
00162         self.robot.goInitial()
00163 
00164         # === TASK-6.1 ===
00165         # Overwrite previous command, for torso using setJointAnglesOfGroup
00166         self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R,
00167                                          wait=False)
00168         time.sleep(1)
00169         self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
00170 
00171         self.robot.goInitial(5)
00172 
00173         # === TASK-6.2 ===
00174         # Overwrite previous command, for arms using setTargetPose
00175         _X_EEF_OVERWRITE = 0.05
00176         _Z_EEF_OVERWRITE = 0.1
00177         _TIME_EEF_OVERWRITE = 7
00178         _POS_L_INIT[0] += _X_EEF_OVERWRITE
00179         _POS_L_INIT[2] += _Z_EEF_OVERWRITE
00180         self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_EEF_OVERWRITE)
00181         self.robot.waitInterpolationOfGroup('larm')
00182         # Trying to raise rarm to the same level of larm.
00183         _POS_R_INIT[0] += _X_EEF_OVERWRITE
00184         _POS_R_INIT[2] += _Z_EEF_OVERWRITE
00185         self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_EEF_OVERWRITE)
00186         self.robot.waitInterpolationOfGroup('rarm')
00187         time.sleep(3)
00188         # Stop rarm
00189         self.robot.clearOfGroup('rarm')  # Movement should stop here.
00190 
00191         # === TASK-7.1 ===
00192         # Cover wide workspace.
00193         _TIME_COVER_WORKSPACE = 3
00194         # Close to the max width the robot can spread arms with the hand kept
00195         # at table level.
00196         _POS_L_X_NEAR_Y_FAR = [0.32552812002303166, 0.47428609880442024, 1.0376656470275407]
00197         _RPY_L_X_NEAR_Y_FAR = (-3.07491977663752, -1.5690249316560323, 3.074732073335767)
00198         _POS_R_X_NEAR_Y_FAR = [0.32556456455769633, -0.47239119592815987, 1.0476131608682244]
00199         _RPY_R_X_NEAR_Y_FAR = (3.072515432213872, -1.5690200270375372, -3.072326882451363)
00200 
00201         # Close to the farthest distance the robot can reach, with the hand kept
00202         # at table level.
00203         _POS_L_X_FAR_Y_FAR = [0.47548142379781055, 0.17430276793604782, 1.0376878025614884]
00204         _RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046, 3.0757659493049574)
00205         _POS_R_X_FAR_Y_FAR = [0.4755337947019357, -0.17242322190721648, 1.0476395479774052]
00206         _RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248, -3.071395243174742)
00207         self.robot.setTargetPose('larm', _POS_L_X_NEAR_Y_FAR, _RPY_L_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
00208         self.robot.setTargetPose('rarm', _POS_R_X_NEAR_Y_FAR, _RPY_R_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
00209         self.robot.waitInterpolationOfGroup('larm')
00210         self.robot.waitInterpolationOfGroup('rarm')
00211         time.sleep(3)
00212         self.robot.setTargetPose('larm', _POS_L_X_FAR_Y_FAR, _RPY_L_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
00213         self.robot.setTargetPose('rarm', _POS_R_X_FAR_Y_FAR, _RPY_R_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
00214         self.robot.waitInterpolationOfGroup('larm')
00215         self.robot.waitInterpolationOfGroup('rarm')
00216 
00217         self.robot.goInitial()