G-ROBOTS クラスマニュアル
Simple Wiki Based Contents Management System
ソフトウェア関連 >> ライブラリ >> G-ROBOTS GR-001コントロールライブラリ >> G-ROBOTS クラスマニュアル

SerialPort.h

このファイルでは、シリアルポートを介した通信を実現するための基本的な機能を実装したSerailComクラスを定義している。

class SerialCom

【クラスの概要】
このクラスは、シリアルポートで通信をするためのクラスである。
通信パラメータは、データ長:8bit、パリティ:None、ストップビット:1と固定されている。通信速度と通信モード(同期/非同期)のみ設定可能になっており、デフォルトでは、通信速度:115200 bps, 通信モード:同期型となっている。
多くのシリアルポートを使ったロボット制御では、同期型が使われているので、非同期モードの通信については、十分にテストされていないことに注意。
【メンバ変数】
Public型:
char *device;
シリアルポートでバイス名。多くのUNIX系のシステムでは、/dev/ttyS0 であり、Windowsでは、COM1 のような文字列になる。
int baudrate;
シリアルポートの通信速度。B0, B9600, B115200など、動作するOSによって、システムヘッダーに定義されている。Windowの場合には、この定義がないので、SerialPort.hに定義している。
int mode;
シリアルポートの通信モード。mode=1 の場合は、非同期入出力モードとなる。通常は、同期モードで実施する方がよい。この実装では、非同期入出力モードに関して十分なテストがなされていない。
HANDLE handle;
シリアルポートの通信ハンドラ。UNIX系のシステムでは、ファイルディスクプリタを表すint型になる。
【コンストラクタとデストラクタ】
コンストラクタ
コンストラクタは、2つ用意されている。引数なしのコンストラクタでは、シリアルでバイスがNULLに設定されてしまうため、通信ポートを開く前にメンバ関数setDevPortを用いてシリアルポート名の設定する必要がある。
SerialCom(); 
SerialCom(char *devname);
デストラクタ
デストラクタでは、通信ポートのクローズを行い、シリアルポート名の削除を行う。
~SerialCom();
【メンバ関数】
Public型:
void setDevPort(char* devname);
シリアルポート名の設定。引数で与えられた文字列を複製し、メンバ変数deviceに代入する。
int openPort();
シリアルポートをオープンし、通信可能な状態にする。オープンするデバイス名は、メンバ変数deviceで設定されたものである。シリアルポートのオープンに失敗した場合には、-1を返す。成功した場合には、メンバ変数 handleを整数にキャストして返す。
void closePort();
シリアルポートをクローズし、メンバ変数handleにH_NULLを代入する。
int Read(char *data, int len);
この関数は、シリアルポート通信の低レベルのデータ受信関数である。シリアルポートから長さlenのデータを読み込み、そのデータをdataの格納して返す。この関数の返り値は、読み込んだデータのバイト数になる。読み込みに失敗した場合には、-1を返す。この関数は、UNIX系のOSのread関数と同等である。
int Write(char *data, int len);
この関数は、シリアルポート通信の低レベルのデータ送信関数である。シリアルポートへ長さlenのデータdataを書き込む。この関数の返り値は、書き込んだデータのバイト数になる。読み込みに失敗した場合には、-1を返す。この関数は、UNIX系のOSのwrite関数と同等である。
int recieveData(char *data, int len);
この関数は、シリアルポートの高レベルデータ受信関数である。シリアルポートからlenバイトデータを読み込み、dataに格納する。返り値は、受信したデータのバイト数であり、失敗すれば-1を返す。この関数は、データを受信する前に、メンバ関数checkBufferにより、受信データがあるかどうかを確認する。もし、受信バッファにlenバイトの受信データはがない場合には、1m秒のスリープ状態に入り、100回連続で受信バッファのデータがlenバイトを満たさない時には、受信バッファの内容を削除し、−1を返す。
受信バッファにlenバイト以上データがあれば、lenバイト受信バッファからデータを読み込み、dataに格納してそのデータのバイト数を返す。
int sendData(char *data, int len);
この関数は、シリアルポートの高レベルデータ送信関数である。引数dataの内容をlenバイトだけシリアルポートに書き込み、書き込んだデータのバイト数を返す。何らかの理由で書き込みに失敗した場合(メンバ関数Writeが-1を返したとき)エラーメッセージを表示し、−1を返す。
int chkBuffer();
この関数は、シリアルポートの受信バッファにあるデータの長さを調べるための関数である。返り値は、受信バッファ内にあるデータのバイト数である。
int clearBuffer();
この関数は、シリアルポートの受信バッファの内容をすべて消去する。返り値は、削除したデータのバイト数であり、失敗すれば−1を返す。
void printPacket(char *data, int len);
標準エラー出力に、dataの内容をlenバイトだけ出力する。出力は、16進数形式で表示される。このメンバ関数は、sendData関数等で失敗した時に呼ばれている。
int getBaudrate();
この関数は、シリアルポートの通信速度を返す。実装では、メンバ変数baudrateを返しているだけである。
int setBaudrate(int rate);
このメンバ関数は、シリアルポートの通信速度をrateに変更する。すなわちメンバ変数baudrateにrateを代入し、その亜多雨を返す。
シリアルポートが既に接続されており、通信状態であればその通信速度を変更する。
int getMode();
この関数は、シリアルポートの通信モードを表すメンバ変数modeの値を返す。
int setMode(int m);
この関数は、シリアルポートが通信状態でないときには、シリアルポートの通信モードを表すメンバ変数modeに引数mの値を代入し、その値を返す。シリアルポートが既に通信状態であれば、エラーメッセージを標準エラー出力に出力し、現在のメンバ変数modeの値を返す。
int isConnected();
この関数は、シリアルポートが通信状態であるかどうかを確認する。通信状態であれば、1をそうでなければ -1 を返す。

SerialRobot

このファイルでは、シリアルポートを介して動作制御を行うロボットの基本的な機能を提供するクラスを定義している。定義したクラスは、下記の3つのクラスである。
  • RobotPostureクラス: ロボットの姿勢(各関節角の列)を表すクラスである。
  • RobotMotionクラス: ロボットの動作を表すクラスであり、RobotPostureの列を表す。
  • SerialRobotクラス: シリアルポートを用いた基本的な動作制御を行うためのクラス。

C言語の関数

short rad2deg(double d);
ラジアン→度への単位変換。 d*180/PI の結果を返す。
double deg2rad(short d);
度→ラジアンの単位変換。 d*PI/180の結果を返す。
THREAD_FUNC thread_execution(void *args);
SerialRobotクラスのスレッドのメイン関数。SerialRobotのインスタンスを引数として、メンバ関数svcを呼び出すループを実行する。

C++のクラス

class RobotPosture

【クラスの概要】ロボットの姿勢を表すクラス。ロボットの各関節の角度およびその角度へ移動するまでの時間を保持する。
【メンバ変数】
Public型:
int numJoints;
この変数は、関節の数を表す。
int *jointAngles;
この変数は、関節角の配列を表し、配列の長さはnumJointsになっている。格納されるデータの単位は、度である。
double *jointAnglesRad;
この変数は、関節角の配列を表し、配列の長さはnumJointsになっている。格納されるデータの単位は、ラジアンである。
double motionTime;
この変数は、jointAnglesの姿勢までに移動するための時間を表している。単位は、ミリ秒とする。
【コンストラクタとデストラクタ】
コンストラクタ
コンストラクタは、ロボットの関節角数nを引数とする。コンストラクタでは、関節角を保持するためのメンバ変数の配列jointAngles, jointAnglesRadの領域確保とnumJointsの設定を行う。また、motionTimeは、100に設定されるため、必要に応じてインスタンス生成後に設定する必要がある。
 RobotPosture(int n);
デストラクタ
デストラクタでは、メンバ変数の配列jointAngles, jointAnglesRadの解放を行う。
 ~RobotPosture();
【メンバ関数】
Public型:
RobotPosture **dupPosture();
新たにRobotPostureのインスタンスを生成し、メンバ変数motionTime, jointAngles, jointAnglesRadをコピーして、そのインスタンスを返す。すなわち自身の複製を生成する。
double setMotionTime(double n);
メンバ変数の配列jointAnglesへ移動する時間を表すメンバ変数motionTimeにnを代入し、その値を返す。すなわち、motionTime=n;が実行される。
int getDegree(int id);
引数 idで指定された度数表現の関節角の値を返す。ただし、idは、1からnumJointsの間の整数である。すなわち、jointAngles[id -1]を返す。
double getRad(int id);
引数 idで指定されたラジアン表現の関節角の値を返す。ただし、idは、1からnumJointsの間の整数である。すなわち、jointAnglesRad[id -1]を返す
void setDegree(int id, int deg);
引数 idで指定された度数表現の関節角を引数degに設定する。ただし、idは、1からnumJointsの間の整数である。すなわち、jointAngles[id -1] = degを実行する。
void setRad(int id, double rad);
引数 idで指定された度数表現の関節角を引数radに設定する。ただし、idは、1からnumJointsの間の整数である。すなわち、jointAnglesRad[id -1] = radを実行する。
void printPosture();
メンバ変数jointAnglesの内容を標準エラー出力に出力する。
void printPosture(std::ofstream& o);
メンバ変数jointAnglesの内容をファイルストリームoに出力する。
void copyPosture(RobotPosture *p);
引数pで与えられたRobotPostureに、メンバ変数motionTime, jointAngles, jointAnglesRadの内容をコピーする。もし、pの関節角数が、自身の関節角数よりも少ない場合には、jointAngles, jointAnglesRadにコピーされる関節角は、0からp->numJoints-1までの要素である。ここで注意するのは、pの関節角数は変更されないということである。
void copyPosture(RobotPosture *p, int offset);
引数pで与えられたRobotPostureに、メンバ変数motionTime, jointAngles, jointAnglesRadの内容をコピーする。このとき、jointAngles, jointAnglesRadにコピーされる関節角は、引数offsetから始まる要素である。ここで注意するのは、pの関節角数は変更されないということである。
int *getJointAngles();
この関数は、内部変数であるjointAnglesを返す。
double *getJointAnglesRad();
この関数は、内部変数であるjointAnglesをラジアン表現に変換し、内部変数であるjointAnglesRadへ代入しそれを返す。
bool equalTo(RobotPosture *pos);
この関数は、引数posで与えられた関節角列であるjointAnglesと自身の内部変数であるjointAnglesの各要素を比較して、すべてが一致している場合に、trueを返し、それ以外であればfalseを返す。
bool nearTo(RobotPosture *pos, int delta);
この関数は、引数posで与えられた関節角列であるjointAnglesと自身の内部変数であるjointAnglesの各要素を比較して、その差がdeltaより小さい場合に、trueを返し、それ以外であればfalseを返す。

class RobotMotion

【クラスの概要】
このクラスは、RobotPostureの列として表現されたロボットの動作を規定するために定義されている。メンバ変数として、vector型のmotion、ロボットの関節角数であるnumJoints、現時点のRobotPosureを表すcurrent、動作の評価順を規定するreverseをもつ。このクラスのインスタンスを用いて、ロボットの動作を定義する。
【メンバ変数】
Public型:
std::vector<RobotPosture *> motion;
この変数は、ロボットの動作を表す。RobotPostureを要素に持つvector型で定義冴えている。
int numJoints;
この変数は、ロボットの関節角数を表す。
int current;
この変数は、ロボットの姿勢列の現在の位置を表す。この変数は、メンバ関数next を実行するときのインデックスとなる。
bool reverse;
この変数は、ロボットの動作を実行する場合に、motionの評価をする場合の順序を規定する。この変数が真であれば、メンバ関数nextで逆順にインデックスが進む。
【コンストラクタとデストラクタ】
コンストラクタ
コンストラクタでは、メンバ変数の初期化を行う。このコンストラクタでは、関節角数は0に設定されるため、メンバ関数setJointsを用いてロボットの関節角数を指定しなければならない。
 RobotMotion();
デストラクタ
メンバ変数motionの要素をすべて削除する。
 ~RobotMotion();
【メンバ関数】
Public型:
int setJoints(int n);
メンバ変数numJointsにn を代入し、その値を返す。
int getSize();
メンバ変数motionのサイズを返す。この値は、ロボットの動作のキーポーズの数に一致している。
int setReverse(bool f);
メンバ変数reverseにfを代入し、その値を返す。
int reset();
登録されている動作のキーポーズの位置(インデックス)を表すメンバ変数currentをリセットする。メンバ変数reverseが偽の場合には、current=0 となり、真の場合には、キーポーズの最後の要素を指すインデックス、すなわち、current = motion.size() -1 に設定される。
RobotPosture *next();
登録された動作の中のcurrentで表されたインデックスの要素である姿勢を返し、インデックスcurrentを1つ進める。メンバ変数reverseが真の場合には、逆方向になるためcurrentを1つ減じる。
RobotPosture *get(int nth);
登録された動作の中の先頭からnth番目の要素である姿勢を返す。
RobotPosture *rget(int nth);
登録された動作の中の末尾からnth番目の要素である姿勢を返す。すなわち、この関数は、get(motion.size() – nth) と同値である。
bool appendPosture(RobotPosture *js);
メンバ変数motionの末尾にjsを追加し、tureを返す。
bool insertPosture(RobotPosture *js, int nth);
メンバ変数motionの先頭からnth番目にjsを追加し、tureを返す。
void deletePosture(int n);
メンバ変数motionの先頭からnth番目の要素を削除する。
bool loadMotionFromFile(const char *name);
nameというファイルからロボットの動作データを読み込み、メンバ変数motionに代入する。ファイルが存在しなかった場合に偽を返す。以前のmotionのデータは、ファイルの読み込み前にすべてクリアされる。
bool saveMotionToFile(const char *name);
メンバ変数motionの内容をnameというファイルに保存する。
bool saveMotionToFile(const char *name,const char *dir);
メンバ変数motionの内容をdirというディレクトリの下にnameというファイルに保存する。
bool loadMotionFromMseqFile(const char *name, SerialRobot *r);
Yaml形式の動作列ファイルnameを読み込み、メンバ変数motionの内容にセットする。このファイルは、Choreonoid で使用されているキーポーズ列と似ているが、各関節角の目標値ではなく、増分をあらわしたものである。
bool loadMotionFromPseqFile(const char *name, SerialRobot *r);
Choreonoid で使用されているYaml形式のキーポーズファイルnameを読み込み、メンバ変数motionの内容にセットする。
bool loadMotionFromYamlFile(const char *name, SerialRobot *r);
Choreonoid で使用されているYaml形式の動作列ファイルnameを読み込み、メンバ変数motionの内容にセットする。
bool saveMotionToPseqFile(const char *name, SerialRobot *r);
メンバ変数motionの内容をChoreonoid で使用されているキーポーズ列のYaml形式に変換してnameというファイルに保存する。(未実装)
bool saveMotionToYamlFile(const char *name, SerialRobot *r);
メンバ変数motionの内容をChoreonoid で使用されている動作列のYaml形式に変換してnameというファイルに保存する。(未実装)
void printMotion();
メンバ変数motionの内容を標準エラー出力に出力する。
void printMotion(std::ofstream& o);
メンバ変数motionの内容をファイルストリームoに出力する。この関数は、メンバ関数saveMotinToFileの内部で呼び出されている。
void clear();
メンバ変数motionの要素をすべて削除する。
bool appendMotion(RobotMotion * rm);
メンバ変数motionの要素に、引数のRobotMotionのメンバ変数motionの内容を追加する。
RobotMotion *dupMotion();
自身のコピーを作成し、それを返す。

class SerialRobot

【クラスの概要】
このクラスは、シリアルポート経由で動作制御を行うロボットの規定クラスである。個々のロボットを制御するクラスは、このクラスの子クラスとして作成する。このクラスでは、ロボットへのコマンド送信および現在の姿勢を取得は、別スレッド(以下、制御スレッドと呼ぶ)を起動し、処理するようになっている。
【メンバ変数】
Public型:
char *name;
ロボットの名前
int joints;
ロボットの関節数
char *servoState;
ロボットの関節サーボモータの状態
RobotPosture *initPosture;
ロボットの初期位置の姿勢。デフォルトでは、すべての関節角を0とする。
RobotPosture *currentPosture;
ロボットの現在の姿勢。ロボットへ各関節角を問い合わせその結果を保持する。
RobotPosture *targetPosture;
ロボットの目標姿勢。このメンバ変数で目標姿勢の設定を行い、メンバ関数startMotionでロボットへ目標姿勢とその動作時間を送信する。
std::string motionDir;
ロボットのモーションファイルが格納されているディレクトリ。モーションの読み込み/書き込みは、このディレクトリの下で行われる。
RobotMotion *motion;
現在のロボットの動作。
Private型:
int threadLoop;
制御スレッドのループに関するフラグ。この値が1(初期値)である時、制御スレッドは動作しており、終了時には個の値を0に変更することで、制御スレッドのループを停止させる。
int motionTime;
このメンバ変数は、デフォルトの目標姿勢への動作時間を表す。単位は、ミリ秒である。この変数を変更したい場合には、メンバ関数setDefaultMotionTimeを使う。
int senseTime;
このメンバ変数は、ロボットへ現在の姿勢を問い合わせる場合のインターバルを表す。単位は、ミリ秒である。本来、高機能の制御装置であれば、制御スレッドの制御時間ごとに現在の姿勢を問い合わせるべきであるが、低速のシリアルインターフェースを使用するロボットでは、毎回すべての関節角の状態の問い合わせには多くの時間を要するため、動作コマンドを送信していない時間に現在の姿勢を問い合わせるように制御スレッドを実装している。
int timeout;
このメンバ変数は、制御ループのインターバルの時間を表す。この値は、連続的な動作の時には、RobotPostureのメンバ変数であるmotionTimeが代入され、通常時は、メンバ変数senseTimeが代入される。
int repeatCount;
このメンバ変数は、ロボットの現在の動作(メンバ変数motion)の繰り返し再生回数を示す。
int reverseFlag;
このメンバ変数は、ロボットの現在の動作(メンバ変数motion)の再生時に、逆向きに再生するかどうかを設定する。
bool executeMotion;
このメンバ変数は、ロボットの現在の動作(メンバ変数motion)の再生中かどうかを表す。
int commandCount;
このメンバ変数は、制御ループにおいてロボットの動作指令(targetPosture)を送信する残り回数を表している。すなわちこの変数が、0以上の時、targetPostureに格納された姿勢へ動くためのコマンドがロボットへ送信される。制御ループでは、1つのコマンドを送信終了後、この変数を1減じる。
char *commandBuf;
この変数は、ロボットへのコマンド送信のためのバッファである。制御スレッド内では、メンバ変数targetPostureへ移動するコマンドを変換後、この変数にコピーしてロボットへ送信する。デフォルトでは、この変数の長さは最大128バイトとしている。
int commandSize;
この変数は、ロボットへの送信するコマンドの長さを表す。すなわち、メンバ変数commandBufに格納されたロボットへのコマンドの長さになる。
THREAD_HANDLE hThread;
制御スレッドへのハンドラ。
MUTEX_HANDLE mutex_com;
ロボットへコマンドを送信する場合のシリアルポートを排他制御するためのミューテックス。
MUTEX_HANDLE mutex_motion;
ロボットへの送信するコマンドバッファ(メンバ変数commandBuf)の操作など、制御スレッド内で参照、変更を排他制御するためのミューテックス。
SerialCom *com;
ロボットへコマンド送信するためのインタフェース。
【コンストラクタとデストラクタ】
コンストラクタ
コンストラクタでは、ロボットを接続しているシリアルポート名と関節角数を引数として与えなければならない。この引数に従って、メンバ変数の初期化を行う。
 SerialRobot(char *devname, int n);
デストラクタ
デストラクタでは、制御スレッドを停止し、その後、シリアルポー卜の削除、RobotMotionの削除、初期化姿勢、現在の姿勢、目標姿勢などをメンバ変数の削除を行う。
 ~SerialRobot();
【メンバ関数】
Public型:
int openPort();
ロボットと通信するためのシリアルポートを開く。内部では、com->openPort()を実行し、その結果を返す。
closePort();
ロボットと通信するためのシリアルポートを閉じる。内部では、com->closePort()を実行する。
void setDevice(char *devname);
ロボットと通信するためのシリアルポート名をdevnameに設定する。内部では、com->setDevPort(devname)が実行される。
void startMotion();
メンバ変数commandCountを1に設定する。これによって制御スレッドでは、targetPostureの姿勢へ移動するためのコマンドを生成してロボットへ送信することができる。
int initPosition();
ロボットの初期姿勢であるinitPostureをtargetPostureにコピーし、メンバ関数startMotionを実行する。これによって、ロボットを初期姿勢にすることができる。
int appendCurrentPosture();
現在の姿勢(メンバ変数currentPosture)を動作motionの末尾に追加する。動作の時間は、メンバ変数motionTimeとする。
int appendCurrentPosture(double mtime);
現在の姿勢(メンバ変数currentPosture)を動作motionの末尾に追加する。動作の時間は、mtimeとする。
int loadMotion(char *name);
メンバ変数motionDirの下でファイルname.m, name.yaml, name.pseq, name.msepの順で検索し、ファイルが存在すれば、そのファイルから動作を読み込み、メンバ変数motionに入れ、motionの長さを返す。どのファイルも存在しない場合は、-1を返す。このとき、motionはクリアされている。
int loadMotionFromM(char *fname);
メンバ変数motionDirの下でファイルfname.ファイルから動作を読み込み、メンバ変数motionに入れ、motionの長さを返す。このファイルは独自形式であり、読み込みに失敗した場合は -1を返す。このとき、motionはクリアされている。
int loadMotionFromYaml(char *fname);
メンバ変数motionDirの下でファイルfname.ファイルから動作を読み込み、メンバ変数motionに入れ、motionの長さを返す。このファイルはChoreonoidの動作列をあわすYaml形式のファイルであり、読み込みに失敗した場合は -1を返す。このとき、motionはクリアされている。
int loadMotionFromPseq(char *fname);
メンバ変数motionDirの下でファイルfname.ファイルから動作を読み込み、メンバ変数motionに入れ、motionの長さを返す。このファイルはChoreonoidのキーポーズ列をあわすYaml形式のファイルであり、読み込みに失敗した場合は -1を返す。このとき、motionはクリアされている。
int loadMotionFromMseq(char *fname);
メンバ変数motionDirの下でファイルfname.ファイルから動作を読み込み、メンバ変数motionに入れ、motionの長さを返す。このファイルはChoreonoidのキーポーズ列をあわすYaml形式のファイルと似ているが、関節角の目標値ではなく関節角の増分がきさいされている。読み込みに失敗した場合は -1を返す。このとき、motionはクリアされている。
int saveMotionToM(char *fname);
現在登録されている動作(メンバ変数motion)をファイルに保存する。
int saveMotionToYaml(char *fname);
現在登録されている動作(メンバ変数motion)をChoreonoidで使われている動作列を表すYaml形式ファイルに変換し、保存する。(未実装)
int saveMotionToPseq(char *fname);
現在登録されている動作(メンバ変数motion)をChoreonoidで使われているキーポーズ列を表すYaml形式ファイルに変換し、保存する。(未実装)
int setJoint(unsigned char id, short deg);
目標姿勢のメンバ変数targetPostureに対してid に対応する関節角にdeg度の値を設定する。idは、1からnumJointsまでの整数としている。
int setMotionTime(int tm);
メンバ変数targetPostureの移動動作時間をtmにする。tmは、msecの単位で指定されている。このデフォルト動作時間は、RobotPostureのmotionTime=0のときに使用される。
void clearMotion();
メンバ変数motionの要素をすべて削除する。
int setDefaultMotionTime(int sval);
メンバ変数motionTimeにsvalを設定し、その値を返す。svalは、msecの単位で指定されており、このデフォルト動作時間は、RobotPostureのmotionTime=0のときに使用される
int getDefaultMotionTime();
メンバ変数motionTimeの値を返す。
int setTimeout(int val);
メンバ変数timeoutにvalを設定し、その値を返す。
int getTimeout();
メンバ変数timeoutの値を返す。
int setCommand(char *packet, int len);
メンバ変数commandBufに、ロボットへのコマンドpacketをコピーし、コマンドの長さlenをcommandSizeに代入する。
int recieveData(char *data, int len);
シリアルポート(ロボット)からのデータを受信し、dataに代入する。データのバイト数は、lenに代入する。
int sendCommand(char *data, int len);
シリアルポート(ロボット)へコマンドdataを送信する。
int startThread();
制御スレッドを開始する。
int stopThread();
制御スレッドを停止させる。すなわち、メンバ変数threadLoopに0を設定し、制御スレッドを終了させる。
int isActive();
制御スレッドの状態(メンバ変数threadLoop)の値を返す。
RobotPosture *getNthPosture(int n);
メンバ変数motionに格納された姿勢列のうち、メンバ変数reverseFlagが真の時には、最後尾からn番目の姿勢を、偽の時には前からn番目の姿勢を返す。nがmotionの長さより大きい場合には、NULLを返す。
int doNthMotion(int n);
メンバ変数motionに格納された姿勢列のうち、メンバ変数reverseFlagが真の時には、最後尾からn番目の姿勢を、偽の時には前からn番目の姿勢を目標姿勢tergetPostureにコピーし、その姿勢に移動する。このとき移動時間は、メンバ変数motionTimeとする。
RobotPosture *getTargetPosture();
目標姿勢を表すメンバ変数tergetPostureを返す。
RobotPosture *getCurrentPosture();
現在の姿勢を表すメンバ変数currentPostureを返す。
RobotPosture *getFirstPosture();
メンバ変数motionに格納された姿勢列のうち、メンバ変数reverseFlagが真の時には、最後尾の姿勢を、偽の時には、最初の姿勢を返す。
RobotPosture *getLastPosture();
メンバ変数motionに格納された姿勢列のうち、メンバ変数reverseFlagが真の時には、最初の姿勢を、偽の時には、最後尾の姿勢を返す。
int isMoving();
メンバ変数commandCountが正の時には1を、それ以外では0を返す。
int setMotionCount(int count);
メンバ変数repeatCountにcountを設定し、その値を返す。
int numJoints();
メンバ変数jointsの値を返す。
char *getServoState();
メンバ変数servoStateを返す。
int setServoState(int id, int state);
メンバ変数servoStateの id-1番目の要素をstateにし、その値を返す。
virtual int checkConnection();
設定しているシリアルポートの確認を行う。接続が成功していれば、0を返すようにする。この関数は、子クラスで上書きすべきである。このクラスでは、常に0を返すように実装されている。
virtual short getAngle(unsigned char id);
idに対応する関節角を返す。関節角の取得に失敗すれば、-1000を返す。このクラスでは、ロボットの実装がないため常に-1000を返す。子クラスを作るときには、必ずoverrideすること。
int connect();
ロボットへのシリアルポートを開いて、接続を確認する。
virtual void getPosture();
ロボットから現在の姿勢を読み込み、メンバ変数currentPostureに代入する。
virtual void svc();
制御スレッド内の制御ループの中で呼び出す。この関数が制御スレッドのメイン関数である。OpenRTM-aistにおけるonExecuteに該当する。デフォルトでは、メンバ変数commandCountが0より大きい時に、次の動作を行う。
1.メンバ変数 executeMotionが真であるとき、メンバ変数motion->next()でロボットの姿勢を獲得し、メンバ変数targetPostureに代入する。
2.メンバ変数 targetPostureをロボットへの命令に変換し、ロボットへ送信する。
3.メンバ変数 commandContを 1 減じる。
メンバ変数 commandContが0以下の場合には、メンバ関数getPosture()を実行し、現在の姿勢をcurrentPostureに格納する。
virtual void postureToCommand(RobotPosture *pos) = 0;
引数で与えられたRobotPostureをロボットへの動作制御コマンドに変換する。変換された制御コマンドはcommandBuf、コマンド長はcommandSizeに代入する。
virtual int jointIdToMotorId(int jid) = 0;
引数で与えられたジョイントIDをモータIDに変換する。この関数は、対象となるロボットに依存する。
virtual int motorIdToJointId(int mid) = 0;
引数で与えられたモータIDをジョイントIDに変換する。この関数は、対象となるロボットに依存する。
virtual int stabilizer() = 0;
目標関節角へ移動する場合、targetPostureに対するキャリブレーションを行うための関数である。この関数は必須ではないが、安定歩行動作等を実施する場合に各自実装すること。デフォルトでは何もしない関数が実装すべきである。
void setMotionDir(const char *dir);
動作を読み込む場合のディレクトリを指定する。すなわち、引数dirはメンバ変数motionDir にコピーされる。

GROBO.h

class GR001

【クラスの概要】
このクラスは、HPIジャパン社製人間型ホビーロボットGR001を制御するためのものであり、前述のSerialRobotクラスの子クラスとして実装されている。
【メンバ変数】
Private型:
RobotMotion *record;
記憶用の動作列。
short *Volts;
各モータの電圧を保持するための配列。
short *Currents;
各モータの電圧を保持するための配列。
unsigned char *MoveJointPacket;
1つのモータに対する動作命令のためのショートパケットのテンプレート。
unsigned char *PosturePacket;
すべてのモータの動作命令のためのロングパケットのテンプレート。
unsigned char *MotorInfoPacket;
1つのモータの状態を取得するためのリクエストパケットのテンプレート。
unsigned char *ServoPacket;
1つのモータのサーボ状態を設定するためのショートパケットのテンプレート。
std::map<std::string, std::vector<int>> m_jointGroups;
関節IDのグループかのためのテープル。(動作未確認)
std::map<std::string, int> Link;
関節IDに名前付けするためのテーブル。(動作未確認)
int freeMotion;
サーボモータに一定の負荷がかかった時、サーボモータをBreakモード(少しトルクがかかった状態だが手で簡単に動かせる状態)に移行するかどうかを表すパラメータ。
【コンストラクタとデストラクタ】
コンストラクタ
コンストラクタは、接続するシリアルデバイス名を与え、メンバ変数Volts, Currents, MoveJointPacket, PosturePacket, MotorInfoPacket, ServoPacketの領域確保と初期化を行う。
 GR001(char *devname);
デストラクタ
デストラクタでは、制御スレッドを停止し、その後、シリアルポー卜の削除、RobotMotionの削除、初期化姿勢、現在の姿勢、目標姿勢などをメンバ変数の削除を行う。
 ~GR001();
【メンバ関数】
Public型:
int connect(int n);
ロボット制御用のシリアルポートへの接続を行う。n=0の場合には、COM0(UNIX系OSでは、/dev/ttyS0)から順にデバイスファイルをオープンし、メンバ関数checkConnectionを用いて接続が確認できれば、デバイス名と通信ハンドラ等を設定し、0を返す。失敗した場合には、−1を返す。
int connect();
ロボット制御用のシリアルポートへの接続を行う。これは、親クラスであるSerialRobotのメンバ関数connect()を呼び出しているのみである。
int checkConnection();
オープンしたシリアルポートに対して{0x41, 0x00}のショートパケットを送信し、0x07が返ってきた場合には、GR001のモータコントローラであると判断し、0を返す。それ以外では、-1を返す。
int printVolts(int flag);
メンバ変数Voltsから、平均のモータ電圧を計算しその値を返す。引数flagが0以外の時には、標準エラー出力に各モータの電圧を出力する。メンバ変数Voltsの更新は、メンバ関数getPostureを実行した時に、同時に取得される。
void printCurrents(int flag);
メンバ変数Currentsを標準エラー出力に出力する。メンバ変数Currentsの更新は、メンバ関数getPostureを実行した時に、同時に取得される。
void printPosture();
ロボットの現在の姿勢を表すメンバ変数currentPostureの要素を標準エラー出力に主力する。
int setTorque(unsigned char id, char val);
idで指定されたモータのサーボの状態を変更する。val=1の場合にはサーボをオンにし、0の場合には、サーボをオフにする。なお、idは、モータIDであり、1からnumJointsまでの整数である。
void setServo(char on, int c);
モータのサーボ状態を変更する。on=1の場合には、サーボオンにし、0の場合には、サーボオフにする。引数cは、モータIDでなく、下記のモータのグループになる。
 c=0  → すべてのモータ
 c=1  → 頭部と胴体のモータ
 c=2  → 右腕のモータ
 c=3  → 左腕のモータ
 c=4  → 両腕のモータ
 c=5  → 右足のモータ
 c=6  → 左足のモータ
 c=7  → 両足のモータ
short getAngle(unsigned char id);
モータID =idのモータの現在の角度を問い合わせその値を返す。問い合わせに失敗した場合は、-254を返す。
int setDegMs(char *data, short deg, unsigned short cs);
モータ制御用コマンドdataに対し、目標角度とその動作時間を設定する。目標角度は、センチ度、動作時間は、センチ秒を単位とする。例えば、90度を100ミリ秒で動作させたい場合には、deg =900, cs = 1000となる。
int moveJoint(unsigned char id, short deg, unsigned short cs);
モータID=idのモータをdegの角度までcs時間で動作させる。このコマンドは、ショートパケットを用いて即座にロボットへ送信される。deg, csに関しては、メンバ関数setDegMsの引数と同じである。
int setJoints(short *deg, int cs);
引数のdegで表された目標角度と動作時間をtargetPacketに設定する。引数の配列degは、ロボットの関節角数以上の長さを持っていなければならない。(エラー処理が省略されている)
int setJointsFromString(char *str);
引数strで指定された目標角度および動作時間をtargetPacketに設定する。Strは、スペースで区切られた整数の列であることを仮定している。また、最初の数は、motionTimeであり、すべての関節角を指定するには、関節角数+1の整数が必要である。
int activateMotionSlot(unsigned char id);
GR001のコントローラにあらかじめ記憶されているidの動作を実行する。(動作未確認)
int activateSenario(unsigned char id);
GR001のコントローラにあらかじめ記憶されているidのシナリオを実行する。(動作未確認)
void postureToCommand(RobotPosture *pos);
引数のRobotPostureposを目標姿勢とするように、GR001の動作コマンドに変換しcommandBufにコマンドの長さをcommandSizeに格納する。GR001の制御CPUのファームウェアが変更された場合には、この関数を修正するのみで動作するはずである。
double *getCurrentJointAngles();
現在の姿勢を取得し、その関節角の列(単位はradian)を返す。ただし、返さえる配列は、モータIDの順に並べてある。
int setTargetJointAngles(double *rad);
ラジアン表現された関節角の列をメンバ変数tragetPostureに設定し、実行する。ただし、与えられる目標関節角の配列は、モータIDの順に並べてあることを前提にしている。
int jointIdToMotorId(int jid);
引数で与えられたジョイントIDをモータIDに変換し、その値を返す。
int motorIdToJointId(int mid);
引数で与えられたモータIDをジョイントIDに変換し、その値を返す。
int stabilider();
目標関節角の補正用関数であるが、現在は何もしない。
int clearRecord();
メンバ変数recordの動作列をクリアする。
int recordCurrentPosture();
現在の姿勢をメンバ変数recordの動作列に追加する。
int doRecord();
メンバ変数recordの動作を再生する。
void getPosture();
ロボットから現在の姿勢を読み込み、メンバ変数currentPostureに代入する。この時メンバー変数freeMotionをチェックし、freeMotion==1の時には、サーボモータに一定以上の負荷が加わったときに、Breakモードに変更し、それ以外であればトルクをオンにする。
bool servo(int jid, bool turnon);
jidのモータの状態を変更する。
bool servo(const char *jname, bool turnon);
jnameで指定されたモータの状態を変更する。
void readJointAngles(double *o_angles);
モータの現在の関節角(メンバ変数currentPosture)をo_angleにコピーする。角度の単はラジアンとする。
void writeJointCommands(const double *i_commands);
i_commadsで指定された目標関節角をメンバ変数targetPostureに設定する。
bool checkEmergency(std::string& o_reason, int& o_id);
モータのエラー状態を検出する。未実装。
void oneStep();
メンバ変数targetPostureを目標姿勢として、動作させる。メンバ関数startMotorと同じ。
bool addJointGroup(const char *gname, const std::vector<std::string>& jnames);
関節のグループ化と名前付けを行う。
int setFreeMotion(int x);
メンバ変数freeMotion に xを代入し、それを返す。
Private型:
bool names2ids(const std::vector<std::string>& i_names, std::vector<int>& o_ids);
i_namesで与えられた関節名をモータIDに変換する。