ディープラーニング用にニューラルネットの汎用プログラムを書いた

ロボットの姿勢制御にディープラーニングを用いたいと思って、その手始めに、ニューラルネットワーク(誤差逆伝搬:バックプロパゲーション)の汎用プログラムを一昨日くらいから熱を込めて書いた。
汎用性を重視して、レイヤーの数や、その中でのニューロンの数を柔軟に変えられるようにすると同時に、C++らしいオブジェクト型のプログラムにしようとした。ようやくバグがないような感じになって、排他的論理和を識別させてみたが、結構学習に手間かかる。
細かくウェイトを変える(グラディエント値の0.05ずつ調整)ようにして、100万個のデータを学習させて認識させた。中間層は4個で、Neuron[0]というのが、最後の出力層のニューロンの値だ。バイアスニューロンをつけていないからかもしれない。
printOutput Neuron [ 0 ]
が、順伝搬の出力値だ。

-----------------No.999975------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986243
教師出力データ 1
-----------------No.999976------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999977------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999978------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986243
教師出力データ 1
-----------------No.999979------------------
入力データ 0 0
Layer::printOutput Neuron [ 0 ] => 0.0137698
教師出力データ 0
-----------------No.999980------------------
入力データ 0 0
Layer::printOutput Neuron [ 0 ] => 0.0137697
教師出力データ 0
-----------------No.999981------------------
入力データ 0 0
Layer::printOutput Neuron [ 0 ] => 0.0137696
教師出力データ 0
-----------------No.999982------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986243
教師出力データ 1
-----------------No.999983------------------
入力データ 1 1
Layer::printOutput Neuron [ 0 ] => 0.0131976
教師出力データ 0
-----------------No.999984------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986243
教師出力データ 1
-----------------No.999985------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999986------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999987------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999988------------------
入力データ 0 0
Layer::printOutput Neuron [ 0 ] => 0.01377
教師出力データ 0
-----------------No.999989------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986244
教師出力データ 1
-----------------No.999990------------------
入力データ 1 1
Layer::printOutput Neuron [ 0 ] => 0.0131984
教師出力データ 0
-----------------No.999991------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986272
教師出力データ 1
-----------------No.999992------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986244
教師出力データ 1
-----------------No.999993------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986273
教師出力データ 1
-----------------No.999994------------------
入力データ 0 1
Layer::printOutput Neuron [ 0 ] => 0.986273
教師出力データ 1
-----------------No.999995------------------
入力データ 1 1
Layer::printOutput Neuron [ 0 ] => 0.013199
教師出力データ 0
-----------------No.999996------------------
入力データ 1 1
Layer::printOutput Neuron [ 0 ] => 0.0131987
教師出力データ 0
-----------------No.999997------------------
入力データ 0 0
Layer::printOutput Neuron [ 0 ] => 0.01377
教師出力データ 0
-----------------No.999998------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986244
教師出力データ 1
-----------------No.999999------------------
入力データ 1 1
Layer::printOutput Neuron [ 0 ] => 0.0131984
教師出力データ 0
-----------------No.1000000------------------
入力データ 1 0
Layer::printOutput Neuron [ 0 ] => 0.986244
教師出力データ 1
-

 

ロボットの姿勢制御にディープラーニング

ロボットを動かす現在のシステムに限界を感じて最初から作り直すことにした。ある意味、原点に帰って、足郎の新しさをちゃんと戦略的に貫くことにある。
(1)外からシステムを押し付けるのではなく、足郎が感じている状況に、足郎自身が対応するようにする。センサーの適切な配置と処理システムが求められる。
(2)そのために、簡単なディープラーニングを取り入れて、与えられた状況の中で、目的適合的な行動を適切に選び出す能力を与える。
(3)サーボを制御するために、全てを動かすのではなく、引きだけを指定してステップ的に動かし、残りは全て完全緩和状態にしてしまう。効率化する。
というわけで、ディープラーニングの基本的内容である、ニューラルネットワークのプログラムを25年ぶりに、C++で書いている。当時は、ただのCだったが。
四半世紀前、経済データの読み取りにニューラルネットワークを使った研究をしていた。ただ、周りからほとんど理解されなかったが。
1。「ニューラルネットワークによる時系列データの特性認識」, 単著, 『経済理論』, 249号, 平成4年9月.
2。「コネクショニズムの経済学---ニューラルネットワークが示唆する新しい経済学」, 単著, 『情報処理技術の応用に関する調査研究』, 第4章,(財)日本情報処理開発協会, (財)関西情報センター,平成5年3月.
3。「環境・エネルギー・成長の経済構造分析---産業連関分析とニューラルネットワーク」, (植田和弘, 長谷部勇一, 寺西俊一, 宮崎誠司, 家田忠と共著),『経済分析』, 経済企画庁経済研究所編, 第134号,平成6年4月.
3番目の論文は経済企画庁経済研究所の正式研究報告書に書かれているものだが、この論文を当時の所長の前で説明したら、鼻であしらわれた。仕方がない。AIがこんなに流行るものだとは、誰も思わなかっただろう。私自身もだが。時代をあまりに先取りすると、辛い目にあうのだ。

倒れるときの加速度センサーデータ、対応猶予時間

センサー(SPI通信)とサーボ制御(C2I通信)が別スレッドで、正常にできる様になった。
COSMで、サーボ制御とセンサー制御の両方のコマンドを新たに加えた。このコマンドで、ロボット歩行中のセンサーデータが取れる様になった。
ロボットが倒れる状況のデータが取れた。正確には、倒れるのを手で支える瞬間のデータが含まれているという意味だが。

こちらが、25秒間、10ミリ秒ごとにデータを取った図である。中央に、全ての軸が上下に大きく揺れているのが倒れるのを支えた瞬間のデータである。
その前後(500個分くらい)だけを表示させると以下の様になる。

こう見ると、倒れそうなことを一番特徴的に予見させるのがZ軸である。つまり、縦方向である。
倒れる状況になったのが、1132時点、つまり動き始めから11秒32の時点である。Z軸がおかしくなり始めたのが1107、つまり11秒07である。したがって、ほぼ250ミリ秒前に倒れそうになり始めたことがわかるのだ。1107の時点では、まだ予見できない。「注意」段階。はっきりと沈み込みがわかるのは、おそらく1116あたりだ。つまり、おかしくなり始めて、90ミリ秒で「倒れそうだ!!」という「警告」が出せそうである。倒れるまで、まだ、160ミリ秒もある。
倒れる160ミリ秒前に警告が出たら、なんとかなりそうなきがする。気象庁の地震や津波や台風の警報の様なものである。
ただ、この図では「どちらに倒れようとしているのか」が、わかりにくい。(1)X軸(+が前方)は、プラス側に偏っているので、前方に倒れそうだというのは、わかるかもしれない。(2)Y軸(+が右側)も、徐々にプラス側になっているので、右側に倒れそうだという予測がつくのかもしれない。(実際、その方に倒れたのだが、笑)
では、このとき何ができるのか?
(1)重心のある足を確認する。これは、本来、足の裏にでも圧力センサーをつけて、確認できる様にしたいが、現時点では安易すぎる解決だ。やはり、動かしているサーボモータの状況から理論的に予想できる「重心足」をチェックする。簡単だ。
(2)経験からだが、重心足の縦方向の姿勢を決定的に決めるのは、人間で言えば、足首の角度である。人間の場合は少し違うが、このロボットの場合は、重心足のくるぶし角度は決定的に重要だ。くるぶしの角度を規定しているのは4つのサーボモータである。これらのサーボ角度を微妙に転倒回避の方向に動かす。
(3)残りの全体もまた、影響する。なるべく、倒れる方向とは逆の方向に重心を持っていける様に調整する。どうするかはまだわからないが。
あと、スレッドが問題である。センサーを常時監視しているスレッドと、サーボモーターを制御しているスレッドが別々だ。センサーを監視しているスレッドが、直接サーボをいじると、トラブルになりそうだ。なるだろう。たとえ、mutexなどで、片方をロックしたりしてもダメだ。だから、サーボを制御しているスレッドが、センサーの警告を受け止める適切なメカニズムを作らなければならない。
センサースレッドが警告情報をどこかに書き込み、サーボスレッドがそれを細かく読んでチェックしているという状況だと、書き込みのバッティングも起こらない様な気がする。

二足歩行中の3軸加速度センサーのデータ

まだ、スレッド化はしていないのだが、センサーのspiによるシリアル通信がサーボを動かすi2cとバッティングしないかを見るために、ロボットが歩行中に、同じRaspberryPiから、センサープログラムも動かして影響を調べた。基本的に影響はない様だ。独立してデータを処理している。
歩行中のセンサーデータを初めてとったことになる。歩行の様子は、前の記事にあるゆっくりとしたものだ。データは20秒間の動きを10ミリ秒間隔で2000個とった。片側3歩くらいずつ動いている。
横方向(Y軸)は、揺れることが必要なので、まあいいが、縦の動き、前後の動き、X軸の動きの不安定さは、歩行の不安定さに直結しているが、これもまた、ある程度までは必要。
実際に速さを増やして、不安定歩行をさせてデータを取らなければならない。

3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす(2)

プログラムの方も形が整ったので、書いておこう。ロボットの頭頂部につけたセンサーをSPIで動かして捉えた結果の図は次のようになった。

Y軸方向(横方向)にロボットを揺らした。結果、X軸方向に細かい揺れも起こっているが、さすがに上下(Z軸方向)には動いていない。縦方向には、重力1(1g)がかかったままである。また、最初から、X軸方向に少し傾きがあることもわかる。

仕様書によると、データは、上記のSDOの様に入ってくる。基本3バイトの最初のバイトは、軸の指定。その後に続く2バイトは軸のデータの上位1バイトと下位1バイトの順に入ってくる。レジスタ二つ使っているので、3バイトのデータを送りながら2倍とずつずらすという作業が必要だ。以下のプログラムでその様にしているのがわかるでしょう。
したがって、C++のint値に変換するには、まず、intでとった上位ビットのデータを4ビットだけ左にシフトさせる。下位ビットは左側に4ビットしかないので、とったintデータを4ビットだけ右にシフトさせて、両者の論理和を取ると一つの整数データになることがわかる。以下のプログラムでそのような作業をしている。
この辺りの複雑さは、I2Cでやってもほぼ同じ。
以下にプログラムを貼り付ける。CとC++がごちゃまぜで、nanosleepなんかも使わなくても良いような気がするが、全部、動かしたまま貼り付けている。
※ 作成にあたって「Raspberry PiでSPI通信」を参照させていただいた。

#include <cstdlib>
#include <iostream>
#include <wiringPiSPI.h>
#include <wiringPi.h>
#include <stdio.h>
#include <time.h>
#include <errno.h>
using namespace std;
//定数定義
#define SS_PORT 8           //GPIO 8
#define SPI_CHANNEL 0       //SPIチャンネル
int main(void) {
    int speed; //通信速度(Hz)
    unsigned char spi_buff[3][3];    //送受信用バッファ
    struct timespec req;
    //sleep設定
    req.tv_sec = 0;
    req.tv_nsec = 130;    // 130ns
    speed = 1000000; //通信速度100kHz
    //バッファ初期化
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            spi_buff[i][j] = 0x0;
        }
    }
    cout << "SPIチャンネル初期化します" << endl;
    int spi_fd = 0;
    if ((spi_fd=wiringPiSPISetup(SPI_CHANNEL, speed)) < 0) {//SPIチャンネル初期化
        printf("wiringPiSPISetup error \n");
        return -1;
    }
    cout << "(使わない)spi_fd = " << spi_fd << endl;
    cout << "PIO初期化します" << endl;
    if (wiringPiSetupGpio() == -1) { //GPIO初期化
        printf("wiringPiSetupGpio error\n");
        return -1;
    }
    pinMode(SS_PORT, OUTPUT); //GPIO8を出力に設定
    digitalWrite(SS_PORT, 1); //SS信号初期化
    unsigned char com[2];
    com[0] = 0x0c;
    com[1] = 0xe3; // デフォルト設定を変更している
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    com[0] = 0x0d;
    com[1] = 0x40; // デフォルト設定のまま
    nanosleep(&req, NULL);
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    int iter = 0;
    int err;
    int xregH, xregL, xout;
    int yregH, yregL, yout;
    int zregH, zregL, zout;
    double xac, yac, zac;
    while (true) {
        //送信用データをバッファにセット
        //printf("******** iter %d: ********** \n", iter);
        for(int i=0;i<3;i++) {
            spi_buff[i][0] = 0x80+2*i;
            spi_buff[i][1] = 0;
            spi_buff[i][2] = 0;
            //printf("write <%d = 0x%x>\n", i, spi_buff[i][0]); //受信データを出力
            digitalWrite(SS_PORT, 0); //SS信号をLOW出力にして通信開始
            err = wiringPiSPIDataRW(SPI_CHANNEL, spi_buff[i], 3); //データ送受信
            if(err == -1){
                cout << "書き込みエラー(1) errno = " << errno << endl;
                break;
            }
            digitalWrite(SS_PORT, 1); //SS信号をHIGH出力にして通信終了
        }
            // デバイスからデータ取得
        xregH = spi_buff[0][1];
        xregL = spi_buff[0][2];
        xout = xregH << 4 | xregL >> 4;
        yregH = spi_buff[1][1];
        yregL = spi_buff[1][2];
        yout = yregH << 4 | yregL >> 4;
        zregH = spi_buff[2][1];
        zregL = spi_buff[2][2];
        zout = zregH << 4 | zregL >> 4;
        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        cout << "No. : " << iter << "  X軸 : " << xac << "   Y軸 : " << yac << "   Z軸 : " << zac << endl;
        iter++;
        delay(10);
        if(iter == 500) break;
    }
    return 0;
}

3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす(1)

今日、ほぼ、丸1日、このことをやっていた。ほとんど進まなかったが、ここにきて光が差してきたので、これまでのことを記録しておく。
KXSD9-2050は、秋月電子のボードになったものだ。これは普通I2Cで使う。いや、これまで、使っていて、なかなかよく加速度を捉えていた。このブログの記事にもロボットの傾きを捉えているデータを何度も掲載してきた。なぜ今更、I2Cを諦めて、SPIでデータを取ろうとしているのか。理由は、I2Cは、サーボモータドライバのPCA9685でも動かしていて、それとどうにも相性が悪いようなのだ。使い方が悪いのかもしれないが。
特に、私としてはセンサーを別スレッドで常時動かして、ロボットの姿勢を監視していたい。揺れを最小限に抑えて、ロボットの早い歩行などに対応したいと思っていのだが、サーボとセンサを別スレッドで動かして、mutexでI2cに片側しかアクセスできないようにロックしても、サーボの角度設定にエラーが出る。センサーが使っている間、サーボを待たせることができないような感じだ。詳しくはわからない。
そこで、センサーの方をI2Cを使わないで、他のGPIOピンからアクセスしたいと思った。そうすれば、二つのスレッドがそれぞれの機器にアクセスするのを許すだろうと思っているからだ。そうならない可能性もあるが、やってみなきゃわからない。
それが理由なのだ。
朝から、それに挑戦したのだが、SPIは、面倒臭い。配線からして分かりにくい。データのやり取りも複雑だ。いや、結局複雑に考えすぎたのかもしれない。
ネット上にそもそも情報が少ない。KXSD9-2050は、SPIでもできるが基本I2Cでやるものだろう。海外サイトも含めてまとまった情報がなかった。だから、今回と次回では、私の様な1日無駄にしてしまう人が少ない様に、親切に記録しておこうと思う。
さて、途中の数え切れない失敗は、省略して、今、うまくいった、いい感じのところだけを記録しておく。なんだか、当たり前の様なことになっているのだが、この当たり前にたどり着くまでの彷徨は、無駄ではなかったと思いたい。
仕様書にある配線図及び秋月電子のマニュアル回路図は次のようなものである。どちらかだけをみても、さっぱりわからないのだ。両方同時に見ないと。

 
したがって、SPIの場合、(チャンネルゼロを使う)

KXSD9  Rspberry PI 3
SCLK (ピン5) SERIAL CLOCK (GPIO 11)
CS (ピン10) CS0 (GPIO 8)
SDO (ピン7) MISO (GPIO 9)
SDI (ピン9) MOSI (GPIO 10)
(ピン1とピン6) 3.3V電源
(ピン2) GND

となる。この辺りでも相当時間かかった!!(記録上はこれが一番大事。あとで、配線どうだったかなと、必ずこの記事を見ると思う)
配線の実際はこの様になっている。RaspberryPIの基盤の上に、専用の追加基盤がはめてある。黄色が2本使ってあって醜いが、線の色不足である。

続いて、プログラムである。配線にも自信がないときには、プログラムのせいでダメなのか、配線がダメなのか、そもそもボードが壊れているのか、あらゆることに疑いが出たが。なんとかなった。
プログラムについては、次回まとめて書く。

足郎1のハードの評価

足郎1は、膝ありロボット「足郎2」への試作品である。足郎2では、膝があるだけではなく、高さは1の倍になる。その大きさを、支えて、動かせるかが問題だ。
その点で、最も重要なのがサーボモータである。
現在使っているのが、KumanのMG996Rだ。現在までの評価は、合格である。価格も、1個600円しかしない。
確かに、原点を抑える安定感も不安があり、サーボ角度の刻みを4度以下だと時々、調整を諦めているふうがあるなど、粗いことは確かだが、支える力はしっかりしている。足郎2で、体躯が2倍になってもなんとかいけそうな気がしている。
また、足郎1は、関節を直接回しているのではなく、関節は自由で、筋肉のように骨格に引っ張り力を与えて動かしているので、この辺りのハードも問題だ。今、引っ張っているのは、0.9ミリのステンレス線だ。これだけの重さと力だと、伸びてしまうのではないかと思っていたが、今の所、そんな様子はない。壊れるのは、接合部分のスッポ抜けみたいなものである。
足郎2では、この線を二本で引くようにするか、もっと太い線を使うけれども、基本的な構造は変えない。
アルミ板の加工を自宅で行なっていたが、設計図を渡して業者にやってもらうか、アキバの共用施設でちまちま作るかのどちらかにして、自宅での作業はやらないようにしたい。クズの処理が大変なので。
RaspberryPi3には、特に問題はない。

足郎1、歩行の改善

これまでの足郎1の歩行は、途中で大きな回転が入ったり不格好だったが、それを改善するために、これまで二足歩行の1サイクルを8フェーズに区切ってプログラムしていたのを6フェーズにして、途中で両足を下ろしたまま揃えるのをやめた。

動画に、歩行の改善が現れていると思う。
今回、超スローにしているが、少しずつ早い動きをつけていきたい。
サーボモータ制御のプログラミング言語名を、Cosmとさらに短くした。
Cosmなしに、ロボットのサーボ制御は、ほぼ不可能な状況だ。今回の動画の動きを作っているスクリプトをいかに示しておく。

##############################
# 2017年5月18日 Toyoaki WASHIDA
# 2017年5月20日 ver.2.2
# 2017年5月21日 ver.2.3
# 2017年5月23日 ver.2.4 ibot拡張子
# 2017年5月23日 ver.2.5
# サーボ制御言語 Cosm ver.1
# 足郎1号の制御のためのプログラム
# 「二足歩行」
# 左右の揺れとともに前への踏み出しが入っている
# ファイル名: ashiro1-5.cosm
##############################
# 角度パラメーター
%param init 0 # 左右に開いている初期状態の角度
%param openfront 50 # 足を前後に開く場合の基準角度
%param shiftbalance 30 # 片足に重心を移動するために傾けるメイン角度
%param support 14 # 開いた時に残った足の角度
%param liftup 6 # 重心が乗った足が、もう一方の足を釣り上げる角度
%param restflat 8 # 釣り上げられた足の角度(水平くらいがいい)
%param rhomb 22  # 菱形に重点移動角度
%param tiptoe 5 # 出した足のつま先
# スピード定義
%defspeed midium {
    interval:3
    steps:40
}
%defspeed slow {
    interval:5
    steps:30
}
# 右へ菱形に潰れる
%defangles rightrhomb {
    RightUpperRight:$rhomb
    RightUpperLeft:-$rhomb
    RightLowerRight:$rhomb
    RightLowerLeft:-$rhomb
    LeftUpperLeft:-$rhomb
    LeftUpperRight:$rhomb
    LeftLowerLeft:-$rhomb
    LeftLowerRight:$rhomb
}
%defangles leftrhomb {
    LeftUpperLeft:$rhomb
    LeftUpperRight:-$rhomb
    LeftLowerLeft:$rhomb
    LeftLowerRight:-$rhomb
    RightUpperRight:-$rhomb
    RightUpperLeft:$rhomb
    RightLowerRight:-$rhomb
    RightLowerLeft:$rhomb
}
# (状態)フェーズ0 直立
# (動作)フェーズ1の状態を作る
%defangles phase0 {
    RightUpperRight:-$liftup
    RightUpperLeft:$liftup
    RightLowerRight:$shiftbalance # 目一杯縮める
    RightLowerLeft:-$shiftbalance
    LeftUpperLeft:$shiftbalance-20 # 少し伸びている
    LeftUpperRight:-($shiftbalance-20)
    LeftLowerLeft:$restflat
    LeftLowerRight:-$restflat
    # 前後は不要か(初期の直立のままでいいから)
}
# フェーズ1
# (状態)右足に重点、左足が右足付近でつり上がっている
# (動作)左足を前側と左側にも開く 左に開くとは初期状態に戻すこと
%defangles phase1 {
    RightUpperRight:$init
    RightUpperLeft:$init
    RightLowerRight:$init
    RightLowerLeft:$init
    LeftUpperLeft:$init
    LeftUpperRight:$init
    LeftLowerLeft:$init
    LeftLowerRight:$init
    # 左足は目一杯開くが、右足は重心があるので、相当残す
    LeftUpperFront:-$openfront
    LeftUpperBack:$openfront
    # 左足のつま先を下げすぎないように-5する
    LeftLowerFront:-$tiptoe
    LeftLowerBack:$tiptoe
    RightUpperFront:$support
    RightUpperBack:-$support
    # ここに+5がないと、重心が後ろ過ぎて、後ろに倒れてしまう
    RightLowerFront:$support+5
    RightLowerBack:-($support+5)
}
# フェーズ2
# (状態)足が左右と前後に開かれている状態、左足が前に出ている
# (動作)股を閉じながら、左足に重心を移動する 右足を釣り上げる
%defangles phase2 {
    LeftUpperLeft:-$liftup
    LeftUpperRight:$liftup
    LeftLowerLeft:$shiftbalance # 目一杯縮める
    LeftLowerRight:-$shiftbalance
    RightUpperRight:$shiftbalance-20 # 少し伸びている
    RightUpperLeft:-($shiftbalance-20)
    RightLowerRight:$restflat
    RightLowerLeft:-$restflat
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$init
    LeftUpperBack:$init
    LeftLowerFront:$init
    LeftLowerBack:$init
    RightUpperFront:$init
    RightUpperBack:$init
    RightLowerFront:$init
    RightLowerBack:$init
}
# フェーズ3
# (状態)左足に重心があり、右足がつり上がっている
# (動作)右足を右側に開きながら、前に踏み出す
%defangles phase3 {
    # 左右は揺れを直立に戻す、ことが開くことになるはず
    RightUpperRight:$init
    RightUpperLeft:$init
    RightLowerRight:$init
    RightLowerLeft:$init
    LeftUpperLeft:$init
    LeftUpperRight:$init
    LeftLowerLeft:$init
    LeftLowerRight:$init
    # 右足を前につりだす
    RightUpperFront:-$openfront
    RightUpperBack:$openfront
    # 左足のつま先を下げすぎないように-5する
    RightLowerFront:-$tiptoe
    RightLowerBack:$tiptoe
    LeftUpperFront:$support
    LeftUpperBack:-$support
    LeftLowerFront:$support+5
    LeftLowerBack:-($support+5)
}
# フェーズ4
# (状態)右足が前に出た状態で、足が開いている
# (動作)左足を右足に近づけながら、重心移動、左足を釣り上げる
%defangles phase4 {
    RightUpperRight:-$liftup
    RightUpperLeft:$liftup
    RightLowerRight:$shiftbalance # 目一杯縮める
    RightLowerLeft:-$shiftbalance
    LeftUpperLeft:$shiftbalance-20 # 少し伸びている
    LeftUpperRight:-($shiftbalance-20)
    LeftLowerLeft:$restflat
    LeftLowerRight:-$restflat
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$init
    LeftUpperBack:$init
    LeftLowerFront:$init
    LeftLowerBack:$init
    RightUpperFront:$init
    RightUpperBack:$init
    RightLowerFront:$init
    RightLowerBack:$init
}
# 実行の定義
# メインルーチンの定義
%defexec walk1 {
    # まず、立たせる
    stand:all
    # 1000ms待機
    delay:1000
    # 右に揺れ始める
    speed:slow
    setAngle:rightrhomb
    setAngle:phase0
    # 歩行のサブルーチンに移動する
    exec:walk2,3
    # 実行が終了したら
    # 緩和状態で立つ
    delay:1000
    relax:all
}
# サブルーチンの定義
%defexec walk2 {
    # 指定されたスピードは再指定がない限り有効
    speed:midium
    # 右足に重心を置いた状態から、左足を前に出す
    setAngle:phase1
    # 左に重点を置いて、右足を釣り上げる
    setAngle:leftrhomb
    setAngle:phase2
    # 右足を前後に開く
    setAngle:phase3
    # 右足に重点を写し、左足を釣り上げる
    setAngle:rightrhomb
    setAngle:phase4
    # 100ミリ秒待機
    # delay:100
}
# exec 実行コマンド
# walk1 定義されている実行内容名
# 1回だけ実行する
%exec walk1 1
######## 以上 ###########

サーボ制御言語COSMの改訂

COSMの文法を少し改定した。
(1)最も大きな改訂は、値にパラメータを利用した数式が使えるようになったこと。数式は全て倍精度(double)で演算され、必要に応じて整数(int)に型変換される。
ex.   10*($spstp + 7)/ $mul
それに伴って、これまで、サーボ角度のパラメータを、ふにする場合、新たにパタメータを定義しなければならなかったが、不要になり、使用時にパラメータの前に '-' (マイナス記号)をつけるだけで良くなった。(構文解析で単項マイナスの処理が面倒なので、冒頭のマイナスの前に0をつけているだけなのだが(笑) -$test の時 0-$testする)
(2)角度 defangles、スピード defspeed、実行 defexec の定義は、キーと値を':'で挟んだものになっているが、値が数式で評価されるために、その中に空白を入れたい場合もある(入れなくてもいいのだが)。その場合は、あたいの全体を [  ] で囲む必要がある。その場合でも':'と [  ] の間に空白を挟んではいけない。
(3)logコマンドの導入。値をログとして出力する(現時点ではコンソールだが、いずれlogファイルへの出力にする)。値のうち  " で挟まれている部分は文字列として出力されるが、それ以外の部分は式として評価され、評価され値が出力される。
今後、まず、変数(%variable)を導入したい。変数は、@xyzのように、@マークで区別するかな。
さらに、この変数が重要な役割を果たす、センサーチェックコマンドを導入する。
センサーのある方向の値がどうなったばいに、角度をどう変化されるとかして、色々な事態に対応できるようにするわけだ。そうなると、比較演算子も必要になるな。
今回の改訂のテストを行ったスクリプトファイルの内容を以下に掲載しておく。

#
# Commandの式評価のテストCosm
# 2017年5月21日
#
 # コメントのテスト0
# パラメータは名前と値で、名前の冒頭は$で始まってはならない、他の文字は大丈夫
# 同じ名前のパラメータの定義があったら、後で定義された方が有効になる
%param test1 8 # コメントのテスト1
%param test2 5
%param inter 9
%param spint 8
%param spstp 5
%param mul 3
%defspeed slow {  # コメントのテスト2
    # キーと値のついになっている、':'の前後に空白があってはならない
    interval:$mul*($spint+2)  # コメントのテスト3
    # 値に空白を入れたい場合は、 [ ]で囲む
    # ただし、':'と[ ]の間に空白があってはならない
    # 値は、いかなる場合も、パラメータを交えた数式として評価される
    # speed の値はどちらも負であってはならない
    # さらにstepsはゼロでもいけない
    steps:[10*($spstp + 7)/ $mul]  # コメントのテスト4
}
%defangles angles1 {
    # 値に空白を入れたい場合は、全体を [ ] で囲むこと
    # ':' の前後に空白は入れない
    lefttopangle:[0.5*($test1 + $test2)]  # コメントのテスト5
}
%defexec exec1 {
    # 次のlogはデバグ済み: " "挟まれていない項は数式として評価される
    # 空白が値にある場合は、全体が [ ] で囲まれている必要がある
    log:["角度 [ "  0.5*($test1 + $test2) " ] を セットします"] # コメントのテスト6
    # 角度もデバグ済み
    setAngle:angles1  # コメントのテスト7
    # delayの値は式として評価される、[ ]の件などは同じ
    delay:$inter*0.6  # コメントのテスト8
    speed:slow  # コメントのテスト9
}
%exec exec1 1 #  # コメントのテスト10