VENUS ROBOTIX

　それではソースコードを解説します。

• 1, 9行目

数値計算のためにmath.hをインクルードします。

また、mah.hでπ(M_PI)を使用するために_USE_MATH_DEFINESをデファインします。

• 5-7行目

シリアル通信のためにインクルードします。

• 11行目

  シリアル通信用のファイルディスクリプタをグローバルで定義します。

• 13-16行目

逆運動学の計算に必要なボディーパラメータを格納する変数をグローバルで定義します。

shin_length： すねリンクの長さ mmを格納します。

thigh_length： ももリンクの長さ mmを格納します。

foot_length： 脚リンクの長さ(すねリンクの長さ + ももリンクの長さ) mmを格納します。

ready_z： 一般的なヒューマノイドロボットは膝を伸ばしたまま歩くことは出来ないので、適度に膝を曲げる必要があります。

　　　　　　 この値は足裏の高さ mmに相当し、両足を上げることで結果的に膝を曲げることができます。

• 18-39行目

シリアルポートをオープンする関数です。

シリアル通信に関してはSerial Servo RTCsで解説していますので、そちらをご覧ください。

• 41-43行目

シリアルポートをクローズする関数です。

この関数はLinux環境とWindows環境で、コード中のシリアルポートの操作を揃えるために定義しています。

シリアル通信に関してはSerial Servo RTCsで解説していますので、そちらをご覧ください。

• 46-51行目

シリアルサーボのデータを格納する構造体を定義します。

この構造体は以下のようなメンバを持っています。

id： シリアルサーボのIDを格納する変数です。

angle： シリアルサーボの位置(角度)を格納する変数です。

duration： シリアルサーボの移動時間を格納する変数です。

• 53-57行目

複数のシリアルサーボのデータを格納する構造体を定義します。

先ほど定義したServo構造体をstd::vectorクラスに格納します。

この構造体は以下のようなメンバを持っています。

num： 指令を与えるシリアルサーボの個数を格納する変数です。

servo： 複数のシリアルサーボに対するデータを格納するstd::vectorクラスです。

• 59-71行目

逆運動学を用いて計算した足関節の角度を格納する構造体を定義します。

この構造体は以下のようなメンバを持っています。

th_1： 股ピッチ関節のシリアルサーボの位置(角度)を格納します。

th_2： 膝ピッチ関節のシリアルサーボの位置(角度)を格納します。

th_3： 足首ピッチ関節のシリアルサーボの位置(角度)を格納します。

また、この構造体はRSシリーズのサーボ用に角度を変換するconver_rsメソッドを持っています。

逆運動学を用いて計算した関節角度はラジアンですので度数に変換します。

さらに、RSシリーズのサーボは0.1 deg単位で指示するので、角度を10倍します。

• 73-107行目

ロボットの動作を行う関数です。

void move_robot(Pose型のデータのリファレンス)

複数のRSサーボにロングパケットモードを用いてGR-001のコントローラRPU-11を通して送信します。

RSシリーズのロングパケットモードは以下の様なパケットを送信します。

[Header] [ID] [Flags] [Address] [Length] [Count] [VID] [Data] [VID] [Data] ・・・ [Sum]

Header： パケットの先頭を表します。ここでは0xFA、0xAFとなります。

ID： 常に0x00を指定します。

Flags： 常に0x00を指定します。

Address： メモリーマップ上のアドレスを表します。Lengthに指定した長さのデータをメモリーマップに書き込みます。

Length： サーボ1つ分のデータの長さ(byte数)を指定します。Length = VID + Dataのバイト数

Count： サーボの数を表します。

Sum： チェックサムです。IDからDataまでの各バイトの排他的論理和(XOR)を計算した値を設定します。

また、RPU-11を通してシリアルサーボに命令を送る場合は、以下のようにHeaderとLengthを頭に付けます。

[RPU header] [Length] [RSシリーズの通常パケット]

RPU header： RS485通信のためのパケットを表します。送信のみの場合は0x53Hを使用します。

Length： RSシリーズの通常のパケットの長さを指定します。

• 75-84行目

std::vectorのインスタンスにパケットを格納します。

各パケットは、push_backメソッドを使用して追加していきます。

2番目のLengthにはパケットの長さを格納します。この時点では長さがわからないので仮の値を格納しています。

• 86-93行目

各シリアルサーボのIDや位置、移動時間を順番に格納します。

std::vectorの値にアクセスするときはatメソッドを使用します。

• 95-100行目

チェックサムを計算し、追加します。

• 102行目

パケットの長さを計算します。

sendbufの長さはsizeメソッドでわかります。

この長さにはRPU headerとLengthも含まれるので、全パケットの長さ - 2の値を格納します。

• 104行目

パケットを送信する前に、送信ポートをクリアします。

• 106行目

コマンドを出力します。

frontメソッドはデータの最初の要素を返します。

先頭要素のアドレスを渡し、sizeメソッドで書き込む長さを指定しています。

• 109-112行目

余弦定理を用いて、3つの辺の長さから内角の値を返します。

内角の大きさ calculation_law_cosines(l_1 mm, l_2 mm, l_3 mm)

各辺と求める角度の関係は以下の様になっています。

l_3で指定した辺の対角αの値が戻り値として返されます。