G-ROBOTS GR-001コントロールライブラリ
Simple Wiki Based Contents Management System
Home Projects Memo Misc Topics Software
ソフトウェア関連 >> ライブラリ >> G-ROBOTS GR-001コントロールライブラリ

はじめに

このドキュメントは、G-ROBOTSシリーズの人間型ロボットであるGR-001をPCから制御するためのライブラリ(以下、G-ROBOTSライブラリ)の概要について解説したものです。
G-ROBOTSは、株式会社HPI Japanから製品化されているホビーロボットです。双葉電子工業株式会社製のモータとコントローラが使用されています。コントローラは、PC からシリアルポートから簡単にアクセスできるようになっています。G-ROBOTSライブラリは、独立行政法人 産業技術総合研究所 原 功主任研究員によって、Web等で公開されている情報を基に実装されています。
また、このライブラリは、シリアルポートで制御するロボット全般にも用意に拡張することができます。実装は、C++で行っており、WindowsでもLinuxでも動作するようになっています。
なお、このドキュメントは、2011年10月13日に公開したバージョンを基に記載されています。

ライブラリのライセンス

G-ROBOTSライブラリは、GNU Lesser General Public License ver.2.1 で配布致します。また、このライブラリを使用していかなる不具合、故障、損害が発生しても一切の責任を負いません。

ライブラリを構成するファイル

  • ヘッダーファイル
    • CUtils.h
      WindowsとLinux間の違いを吸収するためのマクロ定義等を行なっています。
    • YamlParser.h
      LibYAMLというYAML形式のドキュメントパーザーを利用するためのクラスファイルを定義しています。Chorenoidの動作パターンファイルの読み込みに必要なクラスです。
    • SerialPort.h
      シリアルポートの入出力など低レベルの操作を実施するクラスSerialComを定義しています。
    • SerialRobot.h
      GR001をはじめシリアルポートから操作するロボットのためのクラス定義を行なっています。定義されているクラスは、RobotPosture, RobotMotion, SerialRobotの3つです。
    • GROBO.h
      GR001を操作するためのクラスGR001とマクロ変数等を定義しています。このクラスは、SerialRobotクラスの子クラスとして定義されています。
    • stdafx.h
      Visual C++で使われる標準インクルードファイルです。Linuxでは不要です。
    • targetver.h
      Visual C++で使われる最低限のマクロ定義ファイルです。Linuxでは不要です。
  • CPPファイル
    • CUtils.cpp
      G-ROBOTSライブラリで使用されるユーティリティ関数をC言語で実装されています。PthreadとWin32 Threadとの違いを吸収するための関数群も実装されています。
    • YamlParser.cpp
      LibYAMLというYAML形式のドキュメントパーザーを利用するためのクラスを実装しています。このファイルで定義されたクラスは、G-ROBOTSライブラリ特有のものではなく、LibYAMLライブラリを他のC++プロクラムで利用できるように実装されていますが、YAMLドキュメントの読み込みのみに対応しています。
    • SerialPort.cpp
      SerialComクラスの実装を行なっています。このクラスは、WindowsでもLinuxでも利用可能です。
    • SerialRobot.cpp
      シリアルポート接続の多関節型ロボットを操作するためのクラスの実装を行なっています。SerialRobotクラスは、そのままでは利用することが不可能であり、操作対象ロボットに応じて、このクラスを親クラスとしてもつクラスを別途実装剃る必要があります。G-ROBOTSライブラリでは、下のようにGR001クラスを実装しています。
    • GROBO.cpp
      GR001を操作するためのクラスを実装しています。このクラスは、別途公開されているヒューマノイドロボット操作ライブラリhrpsys-baseとの互換性を考慮して実装されています。

ライブラリのクラス構造

このライブラリでは、以下のクラスが用意されている。
SerialCom
シリアルポート操作用のクラス。WindowsとLinuxで動作確認済み。
RobotPosture
ロボットの姿勢(関節角の列)を表すクラス。
RobotMotion
ロボットの動作を表すクラス。ロボットの動作はRobotPostureの列で表されている。
SerialRobot
シリアルポートを用いて制御するロボットの共通クラス。GR001の制御クラスは、このクラスを親として持つ。SerialRobotのクラスは、基本的にコマンド送信用のThreadを生成し、そのThreadは、メンバ変数svc()を周期実行するように設計されています。また、実行周期はメンバ変数timeOutによって制御されている。
GR001
GR001を操作するためのクラス。ロボットの姿勢、動作をフタバ製モータのコマンドに変換等をする。
なお、詳細なクラスマニュアルは、こちらを参照して下さい。

GR001クラスの動作概要

前述のようにGR001を制御するためのクラスは、SerialRobotクラスを継承しています。
GR001のインスタンスの生成には、デバイス名を引数として与えることを前提にしています。デバイス名は、自動的に検索することも可能ですが、複数のGR-001を接続することも想定してクラスを設計しています。
GR001クラスは、SerialRobotクラスと同じようにコマンド送信用Threadを1つ生成し、メンバ変数 commandCountが0以上の場合に、メンバ変数targetPostureと同じ姿勢になるようなコマンドを生成し、GR-001に送信するようになっています。
commnadCountが0以下の場合には、現在の姿勢を取得し、メンバ変数currentPostureに代入するようなコマンドループが実行されています。このとき、現在の姿勢取得には、比較的長い時間がかかるため、Threadの実行周期(メンバ変数timeOutの値)を大きくするようにしています。
また、GR001クラスは、Choreonoidで生成された動作パターンファイルを読み込む機能もあります。デフォルトでは、動作パターンファイルは実行ファイルと同じところにあることを前提にしていますが、メンバ変数motionDirを設定することで任意の場所を動作パターンファイルの格納場所とすることが可能です。

ライブラリのダウンロード

GR-001コントロールライブラリは、GNU Lesser General Public License ver.2.1 で配布致します。また、このライブラリを使用していかなる不具合、故障、損害が発生しても一切の責任を負いません。
ライブラリをダウンロードされた場合には、ライセンスと使用条件に合意されたものとみなします。

添付ファイル:GR001_lib.zip

サンプルプログラム

このライブラリを用いたコマンドライン入力の操作プログラム例を作成してみました。(VC++ 2008で動作確認済み)読んでいただければ、すぐに分かると思いますが、引数でロボットが接続されたシリアルポートのデバイス名を指定します。シリアルポートのデバイス名は、Linuxの場合には、dmesg コマンドで確認することができますが、大抵の場合には "/dev/ttyS0"または"/dev/ttyUSB0"のようになっていると思います。
また、Windowsの場合には、デバイスマネージャーを起動すれば、"ポート(COMとLPT)"のところで確認することができます。
/*
 * Sample Program for GR001:G-ROBOT
 * Copyright(C) all right reserved. 2011 Isao Hara,AIST,Japan.
 *
 */
#ifdef WIN32
#include "stdafx.h"
#endif

#include "GROBO.h"

static int loop = 1;
static GR001 *G_ROBO;

/*
 Signal handler for Ctr-C
 Close com_fd and stop threads
*/
void 
sighandler(int x)
{
  loop = 0;
  G_ROBO->setServo(0, 1);
  delete G_ROBO;
  return;
}

/*
 *  M A I N 
 */
int 
main(int argc, char* argv[])
{
  /*   変数の定義 */
  char line[1024]; 

#ifdef WIN32
   char *devname = "COM1";
#else
   char *devname = "/dev/ttyUSB0";
#endif

  /* Ctrl+C を押したときの処理を登録 */
  signal(SIGINT, sighandler); 

  /* コマンドの引数でシリアルポートを指定する */
  if(argc > 1){
    devname  = (char *)argv[1];
  }

  /* GR001のクラスの初期化とロボットへの接続 */
  G_ROBO = new GR001((char *)devname);
  if( G_ROBO->connect() < 0 ){
    std::cerr << "Error: can't find G-Robot: " <<  devname << std::endl;
  }
  if(G_ROBO->startThread() == 0){
    std::cerr << "Error: fail to create thread" << std::endl;
    delete G_ROBO;
    exit(1);
  }

  /* 動作ファイルのディレクトリを指定する */
  G_ROBO->setMotionDir("motion");
 
  /* コマンド入力ループ */
  while(loop){
    /* コマンド入力プロンプトを出して、一行の入力を待つ */
    std::cout << "G-001 >";
    scanf("%s,line);

   /* 入力されたコマンド文字列に応じて、ロボットを動作させる */

    if(strcmp(line, "On")== 0){                 /* サーボモータをOnにする */
       G_ROBO->setServo(1, 0);
    }else if(strcmp(line, "Off") == 0){         /* サーボモータをOffにする */
       G_ROBO->setServo(0,0);
    }else if(strcmp(line, "Free") == 0){        /* 一定以上の負荷がかかれば、サーボOffになるモードへ移行する */
       G_ROBO->setFreeMotion(1);
    }else if(strcmp(line, "Init") == 0){        /* 初期化姿勢(中腰)になる */
       G_ROBO->initPosition();
    }else if(strcmp(line, "PrintMotion") == 0){ /* 現在の動作を表示する */
       G_ROBO->record->printMotion();
    }else if(strcmp(line, "Start") == 0){       /* 現在の動作を実行 */
       G_ROBO->setMotionCount(1);
    }else if(strcmp(line, "Reverse") == 0){     /* 現在の動作を逆順に実行 */
       G_ROBO->setMotionCount(-1);
    }else if(strcmp(line, "connect") == 0){     /* ロボットへ再接続する */
       G_ROBO->closePort();
       G_ROBO->connect();
    }else if(strcmp(line, "quit") == 0){        /* コマンド入力ループを抜ける(終了) */
       loop = 0;
    }else{                                      /* 入力された文字列の動作パターンファイルを探して、存在すれば実行する */
       std::cout << "Motion = "<< line << std::endl;
       std::string file(line);

       if(G_ROBO->loadMotion((char *)file.c_str()) > 0){
          G_ROBO->setMotionCount(1);
       }
    }

    /* ロボットの姿勢(モータの角度)などを表示する */
    G_ROBO->printPosture();
  }

  /* 終了処理 */
  /** 胸のサーボをオフにする(スイッチを押しやすくするため) */
  G_ROBO->setServo(0, 1);

  /* GR001のクラスを削除する */
  delete G_ROBO;

  std::cerr <<"Terminated." << std::endl;

  exit(0); 
}

参考資料