(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Programming Hiro / NEXTAGE OPEN (RTM)

Description: More OpenRTM-based commands of Hiro (Open) / NEXTAGE OPEN are introduced in scenario-oriented manner. Then how to write your own python script is explained.

Tutorial Level: INTERMEDIATE

Next Tutorial: Programming Hiro/NEXTAGE OPEN in ROS

NOTE: All the tutorials available under the URL http://wiki.ros.org/rtmros_nextage/Tutorials are applicable to the multiple products of Kawada Industries; Hiro (only with the one that opensource software is installed) and NEXTAGE OPEN. To simplify the notation in the rest of the tutorials, we use HiroNXO to appoint the aforementioned robots.

Intro

In one of the previous tutorials we've already gone through operating the robot via python script on ipython terminal. In this tutorial, first we work on more operation based on a sample scenario, which lets you get more sense of the HiroNX python API. Then we look into a python sample code that does operation via HixoNXO python API.

To launch interactive session, try: NEXTAGE:

  • ipython -i `rospack find nextage_ros_bridge`/script/nextage.py     (Simulation)
    
    ipython -i `rospack find nextage_ros_bridge`/script/nextage.py  -- --host nextage    (Real robot example)

Hironx:

  • ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py     (Simulation)
    
    ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py  -- --host hiro014    (Real robot example. Change `host` value accordingly to your robot)

or

hrpsyspy # In this case, the instantiated variable is hcf, not robot

Operation via interactive mode

More in-detail of interactive operation via python I/F

During initialization, robot's client interface class, HIRONX / NextageClient, is instantiated class as a variable robot on ipython terminal. Now see what's available inside of robot by:

In : robot.

You can tab-complete to know what's available:

robot.Groups                            robot.getCurrentRPY                     robot.rh_svc
robot.HandClose                         robot.getCurrentRotation                robot.saveLog
robot.HandGroups                        robot.getJointAngles                    robot.sc
robot.HandOpen                          robot.getRTCInstanceList                robot.sc_svc
robot.InitialPose                       robot.getRTCList                        robot.sensors
robot.OffPose                           robot.getReferencePose                  robot.seq
robot.RtcList                           robot.getReferencePosition              robot.seq_svc
robot.abc                               robot.getReferenceRPY                   robot.servoOff
robot.activateComps                     robot.getReferenceRotation              robot.servoOn
robot.afs                               robot.getSensors                        robot.setHandEffort
robot.checkEncoders                     robot.goActual                          robot.setHandJointAngles
robot.clearLog                          robot.goInitial                         robot.setHandWidth
robot.co                                robot.goOffPose                         robot.setJointAngle
robot.co_svc                            robot.hand_width2angles                 robot.setJointAngles
robot.configurator_name                 robot.hgc                               robot.setJointAnglesOfGroup
robot.connectComps                      robot.ic                                robot.setSelfGroups
robot.connectLoggerPort                 robot.init                              robot.setTargetPose
robot.createComp                        robot.isCalibDone                       robot.setupLogger
robot.createComps                       robot.isServoOn                         robot.sh
robot.el                                robot.kf                                robot.sh_svc
robot.el_svc                            robot.lengthDigitalInput                robot.simulation_mode
robot.ep_svc                            robot.lengthDigitalOutput               robot.st
robot.findModelLoader                   robot.liftRobotUp                       robot.stOff
robot.fk                                robot.loadPattern                       robot.tf
robot.fk_svc                            robot.log                               robot.vs
robot.flat2Groups                       robot.log_svc                           robot.waitForModelLoader
robot.getActualState                    robot.moveHand                          robot.waitForRTCManagerAndRoboHardware
robot.getBodyInfo                       robot.ms                                robot.waitInterpolation
robot.getCurrentPose                    robot.readDigitalInput                  robot.waitInterpolationOfGroup
robot.getCurrentPosition                robot.rh                                robot.writeDigitalOutput

See the list of joint groups and its member links:

In : robot.Groups
Out: 
[['torso', ['CHEST_JOINT0']],
 ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']],
 ['rarm',
  ['RARM_JOINT0',
   'RARM_JOINT1',
   'RARM_JOINT2',
   'RARM_JOINT3',
   'RARM_JOINT4',
   'RARM_JOINT5']],
 ['larm',
  ['LARM_JOINT0',
   'LARM_JOINT1',
   'LARM_JOINT2',
   'LARM_JOINT3',
   'LARM_JOINT4',
   'LARM_JOINT5']]]

An advantage of ipython as a script interpreter is that you can get API info on itself. Suppose you're looking for how to get the current pose but you have no idea of command, first just guess and tab-complete:

In : robot.getCurrent
robot.getCurrentPose      robot.getCurrentPosition  robot.getCurrentRPY       robot.getCurrentRotation

Here you see four options, and clearly getCurrentPose sounds like it. You still need to know the arguments of the method. So put "?" at the end of the command:

In : robot.getCurrentPose?
Type:       instancemethod
Base Class: <type 'instancemethod'>
String Form:<bound method HIRONX.getCurrentPose of <__main__.HIRONX instance at 0x1f39758>>
Namespace:  Interactive
File:       /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys_config.py
Definition: robot.getCurrentPose(self, lname)
Docstring:  <no docstring>

Here you know getCurrentPose takes lname (stands for link name). So run:

In: robot.getCurrentPose('RARM_JOINT0')
Out: 
[0.912826202314136,
 -0.4083482880688395,
 0.0,
 0.0,
 0.39443415756662026,
 0.8817224037285941,
 -0.25881904510252074,
 -0.145,
 0.1056883139872261,
 0.2362568060275051,
 0.9659258262890683,
 0.370296,
 0.0,
 0.0,
 0.0,
 1.0]

This getCurrentPose returns the rotational matrix and the position of the given link in 1 dimensional list. You could also get position only by another method:

In: robot.getCurrent
robot.getCurrentPose      robot.getCurrentPosition  robot.getCurrentRPY       robot.getCurrentRotation
In : robot.getCurrentPosition('RARM_JOINT0')
Out: [0.0, -0.145, 0.370296]

In hrpsys, position vector is presented as a list with three elements [ x, y, z ] where:

 x: forward 
 y: left
 z: upward

Now let's move an arm. First go to initial pose.

In : robot.goInitial()

In : robot.setTargetPose?
Type:       instancemethod
Base Class: <type 'instancemethod'>
String Form:<bound method HIRONX.setTargetPose of <__main__.HIRONX instance at 0x333b758>>
Namespace:  Interactive
File:       /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys/hrpsys_config.py
Definition: robot.setTargetPose(self, gname, pos, rpy, tm)
Docstring:  <no docstring>

gname is the name of the joint group. pos and rpy are in list format. First, store the current values into variables:

In : pos = robot.getCurrentPosition('RARM_JOINT5')
In : rpy = robot.getReferenceRPY('RARM_JOINT5')
In : tm = 3

Current pose should look like:

hiro_before_move_rarm

Then randomly manipulate position, and execute:

In : pos[2] = 0.1

In : robot.setTargetPose('rarm', pos, rpy, tm)
Out: True

Now a plan is successfully shown for the right arm to be moved to the specified pose like this image.

  • NOTE that this image was taken while moveit was active. Ignore that the arm at the starting is displayed in light green, which id because of MoveIt!, and has nothing to do with this tutorial):

hiro_after_move_rarm

After awhile, when you're done with the robot, store both arms back to the power off poses by:

In : robot.goOffPose()

hiro_powerOff

DIO operation (NEXTAGE OPEN only)

Please see rtmros_nextage/Tutorials/Using digital IO (NXO only).

Location of source and document of hrpsys-based API

API document is available separately in the following locations:

  • Due to the limitation that comes from the Python's documentation engine we rely upon, API docs are not mutually linked to each other unlike some other API docs e.g. javadoc.

Programming via Python I/F

Programming HiroNXO's using python can be done via an single integrated interface. Per robot the name of it differs:

* "HIRONX" for Hironx users. * "NextageClient" for NEXTAGE Open users.

In the following, we use "HIRONX" but same things should be possible for NEXTAGE Open users via NextageClient interface.

Sample code (1) Acceptance Test code re-visited

Let's look at a sample code and learn how to write your own application.

In Quick Start Guide, robot owners ran a set of actions. If you're on simulator, please go through the following portion (that is excepted from there) on your simulation environment, to see how it works first (in particular, "acceptance.run_tests_rtm()" is the one we'll look into in this tutorial:

    1. Open one more terminal (this should be at least 3rd terminal opened), then open ipython again:

      $ ipython -i `rospack find hironx_ros_bridge`/scripts/acceptancetest_hironx.py -- --host %HOSTNAME%

      Then on ipython command line, run the sample that does TODO explain.

    2. /!\ The following command runs these tasks via hrpsys interface:

      IN [1]: acceptance.run_tests_rtm()
      You can also run task-by-task:
      IN [1]: acceptance.run_tests_rtm(do_wait_input=True)
      • NOTE: If you're using hironx_ros_bridge version older than 1.0.20 (check by command $ rosversion hironx_ros_bridge), name of the command is different as following:

        $ ipython -i `rospack find hironx_ros_bridge`/test/acceptancetest_hironx.py -- --host %HOSTNAME%
        
        IN [1]: acceptance_test.run_tests_hrpsys()
    3. /!\ The following command runs these tasks via ROS interface:

      IN [1]: acceptance.run_tests_ros()
    4. When you're done with work on ipython, press Ctrl-d to escape its command line.

Let's look into the code.

First, this sample has 2-step dependency as in the image below.

https://docs.google.com/drawings/d/1wNVjZ7LLxJQMpVFkPJeNgMLA99tyJCkQukFo7FnYwXI/pub?w=779&h=282

The actions you just ran in the previous section is written in AcceptanceTest_Hiro class, which uses `AcceptancetestRTM` class to access HIRONX` class. What you really want to understand to write your own program is HIRONX class.

In short, let's look at AcceptanceTestRTM that directly accesses HIRONX.

    • If you're curious, the code of AcceptanceTest_Hiro can be found here. You might want to know about the 7 tasks being run, all of which are kicked from the _run_tests method.

Now here's the entire code:

https://raw.githubusercontent.com/start-jsk/rtmros_hironx/a7a43e5baf4dcd48e34b94f9781defadfbca03d0/hironx_ros_bridge/src/hironx_ros_bridge/testutil/acceptancetest_rtm.py

   1 # -*- coding: utf-8 -*-
   2 
   3 # Software License Agreement (BSD License)
   4 #
   5 # Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
   6 # All rights reserved.
   7 #
   8 # Redistribution and use in source and binary forms, with or without
   9 # modification, are permitted provided that the following conditions
  10 # are met:
  11 #
  12 #  * Redistributions of source code must retain the above copyright
  13 #    notice, this list of conditions and the following disclaimer.
  14 #  * Redistributions in binary form must reproduce the above
  15 #    copyright notice, this list of conditions and the following
  16 #    disclaimer in the documentation and/or other materials provided
  17 #    with the distribution.
  18 #  * Neither the name of TORK. nor the
  19 #    names of its contributors may be used to endorse or promote products
  20 #    derived from this software without specific prior written permission.
  21 #
  22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  33 # POSSIBILITY OF SUCH DAMAGE.
  34 
  35 import time
  36 
  37 from hironx_ros_bridge.constant import Constant
  38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
  39 
  40 
  41 class AcceptanceTestRTM(AbstAcceptanceTest):
  42 
  43     def __init__(self, robot_client):
  44         '''
  45         @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
  46         '''
  47         self._robotclient = robot_client
  48 
  49     def go_initpos(self):
  50         self._robotclient.goInitial()
  51 
  52     def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
  53                          task_duration=7.0, do_wait=True):
  54         '''
  55         @see: AbstAcceptanceTest.set_joint_angles
  56         '''
  57         print("== RTM; {} ==".format(msg_tasktitle))
  58         self._robotclient.setJointAnglesOfGroup(
  59                          joint_group, joint_angles, task_duration, do_wait)
  60 
  61     def set_pose(self, joint_group, pose, rpy, msg_tasktitle,
  62                       task_duration=7.0, do_wait=True, ref_frame_name=None):
  63 
  64         print("== RTM; {} ==".format(msg_tasktitle))
  65         self._robotclient.setTargetPose(joint_group, pose, rpy, task_duration,
  66                                         ref_frame_name)
  67         if do_wait:
  68             self._robotclient.waitInterpolationOfGroup(joint_group)
  69 
  70     def set_pose_relative(
  71                         self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
  72                         msg_tasktitle=None, task_duration=7.0, do_wait=True):
  73         if joint_group == Constant.GRNAME_LEFT_ARM:
  74             eef = 'LARM_JOINT5'
  75         elif joint_group == Constant.GRNAME_RIGHT_ARM:
  76             eef = 'RARM_JOINT5'
  77 
  78         print("== RTM; {} ==".format(msg_tasktitle))
  79         self._robotclient.setTargetPoseRelative(
  80                                     joint_group, eef, dx, dy, dz, dr, dp, dw,
  81                                     task_duration, do_wait)
  82 
  83     def _run_tests_hrpsys(self):
  84         '''
  85         @deprecated: This method remains as a reference. This used to function
  86                      when being called directly from ipython commandline and
  87                      now replaced by optimized codes.
  88         '''
  89         _TIME_SETTARGETP_L = 3
  90         _TIME_SETTARGETP_R = 2
  91         _TIME_BW_TESTS = 5
  92 
  93         self.robot.goInitial()
  94 
  95         # === TASK-1 ===
  96         # L arm setTargetPose
  97         _POS_L_INIT = self.robot.getCurrentPosition('LARM_JOINT5')
  98         _POS_L_INIT[2] += 0.8
  99         _RPY_L_INIT = self.robot.getCurrentRPY('LARM_JOINT5')
 100         self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_SETTARGETP_L)
 101         self.robot.waitInterpolationOfGroup('larm')
 102 
 103         # R arm setTargetPose
 104         _POS_R_INIT = self.robot.getCurrentPosition('RARM_JOINT5')
 105         _POS_R_INIT[2] -= 0.07
 106         _RPY_R_INIT = self.robot.getCurrentRPY('RARM_JOINT5')
 107         self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_SETTARGETP_R)
 108         self.robot.waitInterpolationOfGroup('rarm')
 109         time.sleep(_TIME_BW_TESTS)
 110 
 111         # === TASK-2 === 
 112         self.robot.goInitial()
 113         # Both arm setTargetPose
 114         _Z_SETTARGETP_L = 0.08
 115         _Z_SETTARGETP_R = 0.08
 116         self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
 117                                          dz=_Z_SETTARGETP_L,
 118                                          tm=_TIME_SETTARGETP_L, wait=False)
 119         self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
 120                                          dz=_Z_SETTARGETP_R,
 121                                          tm=_TIME_SETTARGETP_R, wait=False)
 122 
 123         # === TASK-3 ===
 124         # Head toward down
 125         _TIME_HEAD = 5
 126         self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=0.1, tm=_TIME_HEAD)
 127         self.robot.waitInterpolationOfGroup('head')
 128         # Head toward up
 129         self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=-0.2, tm=_TIME_HEAD)
 130         self.robot.waitInterpolationOfGroup('head')
 131         # See left by position
 132         self.robot.setJointAnglesOfGroup('head', [50, 10], 2, wait=True)
 133         # See right by position
 134         self.robot.setJointAnglesOfGroup('head', [-50, -10], 2, wait=True)
 135         # Set back face to the starting pose w/o wait. 
 136         self.robot.setJointAnglesOfGroup( 'head', [0, 0], 2, wait=False)
 137 
 138         # === TASK-4 ===
 139         # 0.1mm increment is not working for some reason.
 140         self.robot.goInitial()
 141         # Move by iterating 0.1mm at cartesian space
 142         _TIME_CARTESIAN = 0.1
 143         _INCREMENT_MIN = 0.0001
 144         for i in range(300):
 145             self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
 146                                              dy=_INCREMENT_MIN,
 147                                              tm=_TIME_CARTESIAN)
 148             self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
 149                                              dy=_INCREMENT_MIN,
 150                                              tm=_TIME_CARTESIAN)
 151             print('{}th move'.format(i))
 152 
 153         self.robot.goInitial()
 154         # === TASK-5 ===
 155         # Turn torso
 156         _TORSO_ANGLE = 120
 157         _TIME_TORSO_R = 7
 158         self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R, wait=True)
 159         self.robot.waitInterpolationOfGroup('torso')
 160         self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
 161  
 162         self.robot.goInitial()
 163 
 164         # === TASK-6.1 ===
 165         # Overwrite previous command, for torso using setJointAnglesOfGroup
 166         self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R,
 167                                          wait=False)
 168         time.sleep(1)
 169         self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
 170 
 171         self.robot.goInitial(5)
 172 
 173         # === TASK-6.2 ===
 174         # Overwrite previous command, for arms using setTargetPose
 175         _X_EEF_OVERWRITE = 0.05
 176         _Z_EEF_OVERWRITE = 0.1
 177         _TIME_EEF_OVERWRITE = 7
 178         _POS_L_INIT[0] += _X_EEF_OVERWRITE
 179         _POS_L_INIT[2] += _Z_EEF_OVERWRITE
 180         self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_EEF_OVERWRITE)
 181         self.robot.waitInterpolationOfGroup('larm')
 182         # Trying to raise rarm to the same level of larm.
 183         _POS_R_INIT[0] += _X_EEF_OVERWRITE
 184         _POS_R_INIT[2] += _Z_EEF_OVERWRITE
 185         self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_EEF_OVERWRITE)
 186         self.robot.waitInterpolationOfGroup('rarm')
 187         time.sleep(3)
 188         # Stop rarm
 189         self.robot.clearOfGroup('rarm')  # Movement should stop here.
 190 
 191         # === TASK-7.1 ===
 192         # Cover wide workspace.
 193         _TIME_COVER_WORKSPACE = 3
 194         # Close to the max width the robot can spread arms with the hand kept
 195         # at table level.
 196         _POS_L_X_NEAR_Y_FAR = [0.32552812002303166, 0.47428609880442024, 1.0376656470275407]
 197         _RPY_L_X_NEAR_Y_FAR = (-3.07491977663752, -1.5690249316560323, 3.074732073335767)
 198         _POS_R_X_NEAR_Y_FAR = [0.32556456455769633, -0.47239119592815987, 1.0476131608682244]
 199         _RPY_R_X_NEAR_Y_FAR = (3.072515432213872, -1.5690200270375372, -3.072326882451363)
 200 
 201         # Close to the farthest distance the robot can reach, with the hand kept
 202         # at table level.
 203         _POS_L_X_FAR_Y_FAR = [0.47548142379781055, 0.17430276793604782, 1.0376878025614884]
 204         _RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046, 3.0757659493049574)
 205         _POS_R_X_FAR_Y_FAR = [0.4755337947019357, -0.17242322190721648, 1.0476395479774052]
 206         _RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248, -3.071395243174742)
 207         self.robot.setTargetPose('larm', _POS_L_X_NEAR_Y_FAR, _RPY_L_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
 208         self.robot.setTargetPose('rarm', _POS_R_X_NEAR_Y_FAR, _RPY_R_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
 209         self.robot.waitInterpolationOfGroup('larm')
 210         self.robot.waitInterpolationOfGroup('rarm')
 211         time.sleep(3)
 212         self.robot.setTargetPose('larm', _POS_L_X_FAR_Y_FAR, _RPY_L_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
 213         self.robot.setTargetPose('rarm', _POS_R_X_FAR_Y_FAR, _RPY_R_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
 214         self.robot.waitInterpolationOfGroup('larm')
 215         self.robot.waitInterpolationOfGroup('rarm')
 216 
 217         self.robot.goInitial()

Let's look at a task as an example.

  • 37 from hironx_ros_bridge.constant import Constant
    38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest

    You realize that HIRONX or NextageClient class, the RTM API we're all interested in, is not imported. No worries, it's taken care of in the following portion:

    43 def __init__(self, robot_client):
    44     '''
    45     @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
    46     '''
    47     self._robotclient = robot_client

    Here, the constructor receives HIRONX and keeps it as a member self._robotclient. So in the rest of this tutorial self._robotclient points to an instance of HIRONX class. The remainder of the code is fundamental methods. For example:

    49 def go_initpos(self):
    50     self._robotclient.goInitial()
    51
    Simple enough, all joints move to the preset initial positions.
    • Later as you progress with your own code, you might want to learn options by looking at API doc of each class and method. In this tutorial we aren't going into detail much so please explore by yourself. For instance goInitial method has these options as follows:

      • def hironx_ros_bridge.hironx_client.HIRONX.goInitial(self, tm = 7, wait = True, init_pose_type = 0)
    Next,
    52 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None, task_duration=7.0, do_wait=True):
    :
    57     self._robotclient.setJointAnglesOfGroup(joint_group, joint_angles, task_duration, do_wait)
    58

    It's also pretty simply calling only the method of HIRONX where a lot of things are already handled so that you can write your code in a very simple way like this.

Sample code (2); Draw a circle with eef

Using the following code, you can draw a circle by the robot's end effector.

/!\ This is just a sample code. You're highly advised to tailor this code and to test virtually before you run it on your real robots.

robot variable needs to be somehow replaced with an instance of your Nextage_Client class.

def circle_eef(radius=0.01, eef='larm', step_degree=5, ccw=True, duration=0.1):
    '''
    Moves the designated eef point-by-point so that the trajectory as a whole draws a circle.

    Currently this only works on the Y-Z plane of *ARM_JOINT5 joint.
    And it's the most intuitive when eef maintains a "goInitial" pose where circle gets drawn on robot's X-Y plane
    (see the wiki for the robot's coordinate if you're confused http://wiki.ros.org/rtmros_nextage/Tutorials/Programming#HiroNXO_3D_model_coordination).

    Points on the circular trajectory is based on a standard equation https://en.wikipedia.org/wiki/Circle#Equations

    @param radius: (Unit: meter) Radius of the circle to be drawn.
    @param step_degree: Angle in degree each iteration increments.
    @param ccw: counter clock-wise.
    @param duration: Time for each iteration to be completed.
    '''
    goal_deg = GOAL_DEGREE = 360
    start_deg = 0
    if eef == 'larm': 
        joint_eef = 'LARM_JOINT5'
    elif eef == 'rarm':
        joint_eef = 'RARM_JOINT5'
    eef_pos = robot.getCurrentPosition(joint_eef)
    eef_rpy = robot.getCurrentRPY(joint_eef)
    print('eef_pos={}'.format(eef_pos))
    X0 = eef_pos[0]
    Y0 = eef_pos[1]
    ORIGIN_x = X0
    ORIGIN_y = Y0 - radius
    print('ORIGIN_x={} ORIGIN_y={}'.format(ORIGIN_x, ORIGIN_y))
    i = 0
    for theta in range(start_deg, goal_deg, step_degree):
        if not ccw:
            theta = -theta
        x = ORIGIN_x + radius*math.sin(math.radians(theta))  # x-axis in robot's eef space is y in x-y graph
        y = ORIGIN_y + radius*math.cos(math.radians(theta))
        eef_pos[0] = x
        eef_pos[1] = y
        print('#{}th theta={} x={} y={} X0={} Y0={}'.format(i, theta, x, y, X0, Y0))
        robot.setTargetPose(eef, eef_pos, eef_rpy, duration)
        robot.waitInterpolation()
        i += 1

Usacase-oriented programming

To not override actions

  • By default, some of the commands in HIRONX class wait for the commanded action ends before the next action starts. Some don't. It depends on the implementation that is clear from the arguments each method takes. Methods with wait argument are capable of waiting or not. Others are not, unless mentioned in their API docs. Example of those that have "wait" feature:

    70 def set_pose_relative(self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, msg_tasktitle=None, task_duration=7.0, do_wait=True):
    73     if joint_group == Constant.GRNAME_LEFT_ARM:
    74         eef = 'LARM_JOINT5'
    75     elif joint_group == Constant.GRNAME_RIGHT_ARM:
    76         eef = 'RARM_JOINT5'
    :
    79     self._robotclient.setTargetPoseRelative(
    80         joint_group, eef, dx, dy, dz, dr, dp, dw, task_duration, do_wait)

    Unless you want, generally it would be safe to set wait=True by default in your codes.

    In the following, setTargetPose doesn't take any wait / interruption signals. In that case you could work it around by calling HrpsysConfigurator.waitInterpolationOfGroup() (HrpsysConfigurator is the parent class of HIRONX).

    61 def set_pose(self, joint_group, position, rpy, msg_tasktitle, task_duration=7.0, do_wait=True, ref_frame_name=None):
    :
    65     self._robotclient.setTargetPose(joint_group, position, rpy, task_duration, ref_frame_name)
    67     if do_wait:
    68         self._robotclient.waitInterpolationOfGroup(joint_group)
    68

Sending continuous trajectory commands

Discussed here. Please ask for the detail there if necessary.

 hcf.playPatternOfGroup('LARM', 
                        [[0.010,0.0,-1.745,-0.265,0.164,0.06],
                         [0.010,-0.2,-2.045,-0.265,0.164,0.06],
                         [0.010,-0.4,-2.245,-0.265,0.164,0.06],
                         [0.010,-0.6,-2.445,-0.265,0.164,0.06],
                         [0.010,-0.8,-2.645,-0.265,0.164,0.06]],
                        [1,1,1,1,1])

hcf can be robot (depends on how you boot python interface).

From hrpsys 315.6.0, you can use setJointAnglesSequenceOfGroup.

Relative movement using setTargetPoseRelative command

Moving EEFs or any joints by passing delta value (change from the current status) is made easy with HIRONX interface.

  • Example1: In the following, [1] moving torso 0.1 rad over 3 second, [2] moving EEF at right hand toward front 10 centimeters:
    In [1]: robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=0.1, tm=3)
    
    In [2]: robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.1, tm=3)

To use your custom Python code that uses hrpsys commands

In this page up to here, we've been talking about how to use the methods provided by the robot's client interface class (HIRONX / NextageClient). There you ran a script hironx.py or nextage.py to instantiate the interface class.

Now, let's assume that you created your own application module by integrating the methods from client interface class. Say you named it your_nxo_sample.py. Now you want to run it against the robot (either on simulation or on the real robot).

  • One way is to simply duplicate nextage.py, and replace the line where NextageClient class is instantiated (also remove the rest of the file where the instance of NextageClient is accessed).

  • The above way not smart nor clean because you simply duplicate the majority of the code, which is a bad practice in software development. As far as the author knows, hrpsys_tools/hrpsys_tools_config.py is intended to serve for this purpose (see also start-jsk/rtmros_common#340 for the discussion when it was created), but unfortunately there has been no report on how to use that with HiroNXO yet (as of 1/30/2017).

Wiki: rtmros_nextage/Tutorials/Programming_Hiro_NEXTAGE_OPEN_RTM (last edited 2017-01-30 21:49:47 by IsaacSaito)