Note: This tutorial assumes that you have completed the previous tutorials: rtmros_nextage/Tutorials/Changing Grippers on Nextage Hardware.
(!) 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.

Adding / Changing Robot Model for HiroNxo

Description:

Keywords: Kawada, Nextage Open, dual-arm

Tutorial Level: ADVANCED

Next Tutorial: Changing Grippers on Nextage Hardware

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.

Introduction

This wiki tutorial describes how to add or modify robot models for HiroNXO, particularly taking an example of creating a software module (models, program) of a custom hand with Nextage Open. The robot's Digital In and Output (DIO) assignment is available through the Robot Hardware Manual.

Adding a custom hand

Prerequisite

Download Source Code

In this tutorial, we will add your custom hand module into nextage_description, the original package of NEXTAGE OPEN that stores model information. Even if the package is already installed on your machine via binary installation (eg. by using apt-get on Ubuntu), modifying to its protected directory is not a good idea at all and thus you're encouraged to download the source.

$ cd %YOUR_CATKIN_WORKSPACE%/src
$ git clone https://github.com/tork-a/rtmros_nextage.git
$ cd %YOUR_CATKIN_WORKSPACE%/
$ catkin_make && source devel/setup.bash

We will assume we use nextage_description and nextage_ros_bridge packages from the downloaded source.

Prepare Hand Model

Let's assume that we have a CAD file we'd like to add as a new hand called SuctionHand.wrl, a VRML file that includes a robot model whose coordinate frame is equivalent to original ChunkHand.wrl.

Sample of NEXTAGE OPEN on Visualizer RViz"

In the factory setting of NEXTAGE OPEN, the initial pose of the arms is like the image above where the upper arm links straight down, links from elbows to hands stretching toward the front of the robot. The absolute coordinate of the robot: x axis points forward from the robot chest, the y axis is to the robot's left, and the z axis is in the vertical up direction. The origin of the hand model is located on the interface flange surface.

World coordinates of hand model

Put the .wrl file under nextage_description/models

Convert To Semantic Model for RTM-ROS world

HiroNXO system involves 2 different model formats in addtition to ROS' standard URDF. Hope this diagram helps to understand the relationship between each.

Diagram of different model files (Original diagram file)

  • VRML format (extension: .wrl) is used by the underlining controller hrpsys

  • COLLADA format is used by IKFast plugin in MoveIt!

First, once again check prerequiste, in particular downloading source code on to your machine.

Add your model to OpenHRP3 wrl model file

(TODO: elaborate why we need to do this, what's OpenHRP3 wrl model, etc.)

Change nextage_description/models/main.wrl as follows:

 # 'Inline {url "ChunkHand.wrl"}'
 'Inline {url "SuctionHand.wrl"}"

The portion that contains the same line of code now looks like this:

 Transform {
  translation  -0.065 0 0 children [
   # Inline { url "ChuckHand.wrl" }
   Inline { url "SuctionHand.wrl" }
  ]
 }

Create COLLADA model

First, make sure you're at the directory where the model files we're editing are.

 $ roscd nextage_description/models

Then, run following commands to convert wrl model into COLLADA format:

 $ rosrun openhrp3 export-collada -i main.wrl -o main.dae
 $ cp main.dae ../../nextage_ros_bridge/models/nextage.dae

Create URDF model

You might need to install collada_urdf package in advance by following:

$ apt-get install ros-%ROS_DISTRO%-collada-urdf
$ apt-get install ros-hydro-collada-urdf

Run following commands to convert COLLADA model into URDF model:

 $ cd ../urdf
 $ rosrun collada_urdf collada_to_urdf ../models/main.dae --mesh_output_dir=meshes --mesh_prefix=package://nextage_description/urdf/meshes
 $ sed s/HiroNX/NextageOpen/ HiroNX.urdf  > NextageOpen.urdf  # NOTE-1
  • NOTE-1. if you want to avoid this, you have to add the following change:
    DEF HiroNX Humanoid               # Current 
    DEF NextageOpen Humanoid          # New

This may have several side-effects, for example we have to change args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0" to NextageOpen(Robot)0 in nextage.py.

Tips

Better to check urdf file with:

$ roslaunch urdf_tutorial display.launch model:=`rospack find nextage_description`/urdf/NextageOpen.urdf gui:=True

You might want to change the fixed frame to WAIST on RViz to show the robot model properly. Then use the slider bars on joint_state_publisher to see if certain joints work as expected.

By the way, this does not work with collada file since joint_state_publisher is not capable of reading it.

.conf and .xml files in conf directory

Once done in generating model files, you need to tell the underlining libraries (OpenRTM, OpenHRP3) about the locations of model files. To do so, you just need to build your package so that the configuration files will be generated using the templates that are placed in conf folder in nextage_ros_bridge package. At this time of writing, you might see the following template files:

RobotHardware.conf.in
conf.in
nosim.RobotHardware.conf.in
nosim.conf.in
nosim.xml.in
xml.in

Now build:

$ cd %YOUR_CATKIN_WORKSPACE%
$ catkin_make

This turns those files with .in suffix into files with concrete path values in them, and .in will be removed from the file names.

Notify the robot about the new model

Simulator and the real robot can apply the change only by the updated model files being placed at the right location.

Notify the simulator

Replace .wrl file in nextage_description/models with the one you just created.

Apply the change to the robot

Prerequiste for updating model files on QNX

First make sure you have a write access to the following folder:

  • /opt/jsk/etc/NEXTAGE if you use NEXTAGE and the version of your hrpsys controller inside of QNX is 315.2.8 or higher.

  • /opt/jsk/etc/HIRONX

    • if you use NEXTAGE and the version of your hrpsys controller inside of QNX is 315.2.7 or lower.

    • if you use Hironx.

Replace models on QNX
  1. Replace .wrl file in the folder your situation corresponds to above, with the one you just created.

  2. Then, reboot QNX.

  3. After reboot, first verify in the controller level: check the following log files and see if they look the same or similar.
    $ cd /opt/jsk/var/log
    $ more /opt/jsk/var/log/NameServer.log   
    
    Thu Oct 16 17:55:16 2014:
    
    Starting omniNames for the first time.
    
    Thu Oct 16 17:55:17 2014:
    
    Wrote initial log file.
    Read log file successfully.
    Root context is IOR:010000002b00000049444c3a6f6d672e6f72672f436f734e616d696e672f4e616d696e67436f6e746578744578743a312e3000000100
    00000000000070000000010102000c00000031302e3235342e32342e31009d3a00000b0000004e616d6553657276696365000300000000000000080000000100
    000000545441010000001c000000010000000100010001000000010001050901010001000000090101000354544108000000840640540100c01a
    Checkpointing Phase 1: Prepare.
    Checkpointing Phase 2: Commit.
    Checkpointing completed.
    
    $ more /opt/jsk/var/log/ModelLoader.log  
    ready
    loading /opt/jsk/etc/HIRONX/model/main.wrl
    Humanoid node
    Joint nodeWAIST
      Segment node WAIST_Link
      Joint nodeCHEST_JOINT0
        Segment node CHEST_JOINT0_Link
        Joint nodeHEAD_JOINT0
          Segment node HEAD_JOINT0_Link
          Joint nodeHEAD_JOINT1
            Segment node HEAD_JOINT1_Link
            VisionSensorCAMERA_HEAD_R
            VisionSensorCAMERA_HEAD_L
        Joint nodeRARM_JOINT0
          Segment node RARM_JOINT0_Link
          Joint nodeRARM_JOINT1
            Segment node RARM_JOINT1_Link
            Joint nodeRARM_JOINT2
              Segment node RARM_JOINT2_Link
              Joint nodeRARM_JOINT3
                Segment node RARM_JOINT3_Link
                Joint nodeRARM_JOINT4
                  Segment node RARM_JOINT4_Link
                  Joint nodeRARM_JOINT5
                    Segment node RARM_JOINT5_Link
                      ForceSensorrhsensor
        Joint nodeLARM_JOINT0
          Segment node LARM_JOINT0_Link
          Joint nodeLARM_JOINT1
            Segment node LARM_JOINT1_Link
            Joint nodeLARM_JOINT2
              Segment node LARM_JOINT2_Link
              Joint nodeLARM_JOINT3
                Segment node LARM_JOINT3_Link
                Joint nodeLARM_JOINT4
                  Segment node LARM_JOINT4_Link
                  Joint nodeLARM_JOINT5
                    Segment node LARM_JOINT5_Link
                      ForceSensorlhsensor
    The model was successfully loaded !
    
    $ more /opt/jsk/var/log/rtcd.log 
    Logger::Logger: streambuf address = 0x805fc70
    hrpExecutionContext is registered
    pdgains.file_name: /opt/jsk/etc/HIRONX/hrprtc/PDgains.sav
    dof = 15
    open_iob - shmif instance at 0x80acf58
    the number of gyros = 0
    the number of accelerometers = 0
    the number of force sensors = 0
    period = 5[ms], priority = 49
    SequencePlayer::onInitialize()
    period = 5[ms], priority = 49
  4. Then verify in the hrpsys level: (TODO how to do so is in discussion https://github.com/start-jsk/rtmros_hironx/issues/314)

  5. Finally, you can check ROS' tf tree by a graphical tool:

    $ rqt &

    Then from Plugin menu, open TF Tree that opens rqt_tf_tree. See if the tree makes sense to you.

Create MoveIt! package containing hands

Unless you want to control the hands with MoveIt!, we do not think anything is needed for using MoveIt! with new hands model.

Create Software In Python

You're almost ready to get your hands moving. Now, coding time.

Hand's software implementation of Nextage employs Command Pattern. With this design, task to add a custom hand software module can be minimized; create Command class where the hands' specific functionalities are defined, and bind it to another class that represents hands, and finally use it from "main" robot client module (eg. nextage_client.py).

Nextage hand class hierchy (Editable original diagram file)

Add Hand's Function By Creating "Command" Class

First, create a python file that extends nextage_ros_bridge/abs_hand_command.py that contains basic stuff for all hand modules, put it in the source folder of nextage_ros_bridge.

$ roscd nextage_ros_bridge/src/nextage_ros_bridge/command
$ touch %YOUR_COMMAND%.py

Then from here, we'll look at a code snippet of a concrete example of suction hand python module (cited Feb 2, 2014):

   1 #!/usr/bin/env python
   2 
   3 import time
   4 import threading
   5 
   6 import rospy
   7 
   8 from abs_hand_command import AbsractHandCommand
   9 
  10 
  11 class AirhandCommand(AbsractHandCommand):
  12     '''dio_writer
  13     Following Command design pattern, this class represents command for
  14     an Airhand of NEXTAGE OPEN.
  15 
  16     As of 2/1/2014, it's only implemented for a right arm (since there's no
  17     testing environment for left arm).
  18     '''
  19     # TODO: Unittest is needed!!
  20 
  21     # For air hands
  22     AIRHAND_DRAWIN = 'drawin'
  23     AIRHAND_KEEP = 'keep'
  24     AIRHAND_RELEASE = 'release'
  25 
  26     ## Might not be necessary. Maybe use only where you have to specify
  27     ## dangerous situation.AIRHAND_KEEP
  28     AIRHAND_DANGER = 'danger'
  29 
  30     def __init__(self, hands, hand):
  31         '''
  32         @see nextage_ros_bridge.command.abs_hand_command.AbsractHandCommand
  33         @type hands: nextage_ros_bridge.base_hands.BaseHands
  34         @type hand: str
  35         @param hand: Side of hand. Variables that are defined in
  36         nextage_ros_bridge.base_hands.BaseHands can be used { HAND_L, HAND_R }.
  37         '''
  38         super(AirhandCommand, self).__init__(hands, hand)
  39         self._SLEEP_POST_RELEASE = 3.0
  40 
  41     def execute(self, operation):
  42         '''
  43         @see abs_hand_command.AbsractHandCommand.execute
  44         '''
  45         dout = []
  46         mask = [self._DIO_SUCTION_R_1, self._DIO_SUCTION_R_2]
  47 
  48         # TODO: Implement for R hand too.
  49         if self.AIRHAND_DRAWIN == operation:
  50             if self._hands.HAND_L == self._hand:
  51                 dout = [self._DIO_SUCTION_L_1]
  52             elif self._hands.HAND_R == self._hand:
  53                 dout = [self._DIO_SUCTION_R_1]
  54         elif self.AIRHAND_KEEP == operation:
  55             if self._hands.HAND_L == self._hand:
  56                 pass  # Do nothing since off for both pins.
  57             elif self._hands.HAND_R == self._hand:
  58                 pass  # Do nothing since off for both pins.
  59         elif self.AIRHAND_RELEASE == operation:
  60             if self._hands.HAND_L == self._hand:
  61                 dout = [self._DIO_SUCTION_L_2]
  62             elif self._hands.HAND_R == self._hand:
  63                 dout = [self._DIO_SUCTION_R_2]
  64 
  65             # Create a thread to do KEEP action after the specified amount
  66             # of time without stopping the program.
  67             thread = AirhandReleaseThread(self, self._SLEEP_POST_RELEASE)
  68             thread.start()
  69         else:
  70             # TODO: Might want to thrown exception?
  71             rospy.logwarn('No gripper specified. Do nothing.')
  72             return
  73         self._hands._dio_writer(dout, mask)
  74 
  75     def _assign_dio_names(self):
  76         '''
  77         @see abs_hand_command.AbsractHandCommand._assign_dio_names
  78         '''
  79         #DIO reassignment for the class-specific purpose
  80         self._DIO_SUCTION_R_1 = self._DIO_22
  81         self._DIO_SUCTION_R_2 = self._DIO_23
  82         self._DIO_SUCTION_L_1 = self._DIO_27
  83         self._DIO_SUCTION_L_2 = self._DIO_28

Now let's look at what this airhand_command.py does step-by-step:

  •   11 class AirhandCommand(AbsractHandCommand):
    

    Extending AbsractHandCommand as already mentioned.

      22     AIRHAND_DRAWIN = 'drawin'
      23     AIRHAND_KEEP = 'keep'
      24     AIRHAND_RELEASE = 'release'
    

    Possible commands are defined as member variables. With the design of this particular class, this command represents multiple functionalities that this hand provides (which, may be a little confusing with respect to the concept of Command Pattern). Related to that, it's more useful to keep these accessible to the external classes (ie. no leading underscore with their names).

      30     def __init__(self, hands, hand):
      31         '''
      32         @see nextage_ros_bridge.command.abs_hand_command.AbsractHandCommand
      33         @type hands: nextage_ros_bridge.base_hands.BaseHands
      34         @type hand: str
      35         @param hand: Side of hand. Variables that are defined in
      36         nextage_ros_bridge.base_hands.BaseHands can be used { HAND_L, HAND_R }.
      37         '''
      38         super(AirhandCommand, self).__init__(hands, hand)
    
    Initializing with just passing arguments to the constructor of its super class.

      75     def _assign_dio_names(self):
      76         '''
      77         @see abs_hand_command.AbsractHandCommand._assign_dio_names
      78         '''
      79         #DIO reassignment for the class-specific purpose
      80         self._DIO_SUCTION_R_1 = self._DIO_22
      81         self._DIO_SUCTION_R_2 = self._DIO_23
      82         self._DIO_SUCTION_L_1 = self._DIO_27
      83         self._DIO_SUCTION_L_2 = self._DIO_28
    

    We skip and reach down the bottom of the code first where we re-assign the name of DIOs to adjust to the specific purpose the DIOs are used for this class. To do so by overriding method _assign_dio_names isn't mandatory (though recommended to avoid confusion of other developers or even you in the future).

    In this particular class, these four DIOs are used for the stated purposes, which are defined in the hand's electrical design (TODO: reference to electrical design tutorial how to assign DIO if any)). Now going back up the code,

      41     def execute(self, operation):
    
    This part is the "body" of the functions your custom hand provides.

      45         dout = []
      46         mask = [self._DIO_SUCTION_R_1, self._DIO_SUCTION_R_2]
    

    Here you prepare two lists: dout and mask. While writing this hand's code, you're freed from bit manipulation although the interface of the robot's DIO is designed to interact by bit arrays. You just have to specify which the specific bits to use. So the two lists are:

    • dout: bit values that indicate on/off of the DIO pins.

    • mask: Represents the enabled pin(s).

      49         if self.AIRHAND_DRAWIN == operation:
      50             if self._hands.HAND_L == self._hand:
      51                 dout = [self._DIO_SUCTION_L_1]
    
    Now the operations are coded. Here we're only looking at the first conditional block; the rest is just in similar structure.

    Simply we assign the bit for the identified side of the hands for the designated operation (draw in in this case).

      73         self._hands._dio_writer(dout, mask)
    

    This line is required; we send out the bit arrays to the DIO interface.

Bind Hand-command to Hand Class

We've added a hand, a model and its function. So now we're good to use it.

First, instantiate the command class you just created in a Hand class. You can look at a sample here, some of which is cited like:

class Iros13Hands(BaseToolchangerHands):
:
    def __init__(self, parent):
        '''
        @see: AbsractIros13Hands.__init__
        '''
        self.airhand_l_command = AirhandCommand(self, self.HAND_L)
        self.airhand_r_command = AirhandCommand(self, self.HAND_R)

Add the side as an argument. That's it for the Hand class.

Very finally, you are all ready to use the hand!

Use Hand-command From Robot Client

From robot's client class, like NextageClient, you can call execute function of each Command class (just like these methods in NextageClient class).

That's it! As I predicted, hand-command is pretty much all the code you need to write in order to add your custom hand module.

Modifying existing 3D models

As explained in the case above, HiroNXO system is based on multiple 3D model formats. This setting comes from the system architecture (ranges over multiple middleware: OpenRTM, OpenRave and ROS). This can be indeed incovenient, and more importantly error-prone especially when adding your custom models / modifying existing models.

For both Hironx and NEXTAGE OPEN robots, the manufacturer provided 3D model in a single format. This single format (collada for Hironx, VRML for NEXTAGE OPEN). To maintain the consistency between model files in different formats, it's the most recommended for you to stick to the originally provided format to add changes (i.e. editing URDF manually is NOT recommended at all). Good thing is that the conversion between model formats is well prepared.

You may also want to consult this informative post for more information about model files relationship for HiroNXO.

Wiki: rtmros_nextage/Tutorials/Changing Nextage Grippers in Robot Model (last edited 2016-11-05 23:55:34 by IsaacSaito)