VENUS ROBOTIX

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

• 1, 6行目

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

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

• 5行目

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

• 8行目

シリアル通信用のハンドルをグローバルで定義します。

• 12-15行目

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

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

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

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

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

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

• 17-76行目

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

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

• 78-81行目

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

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

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

• 83-88行目

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

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

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

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

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

• 90-94行目

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

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

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

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

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

• 96-108行目

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

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

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

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

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

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

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

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

• 110-145行目

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

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シリーズの通常のパケットの長さを指定します。

• 112-121行目

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

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

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

• 123-130行目

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

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

• 132-137行目

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

• 139行目

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

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

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

• 141行目

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

• 143-144行目

コマンドを出力します。

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

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

• 147-150行目

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

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

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

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