足郎2のシミュレーターの第1段階、ほぼ出来上がる

この間、計算に次ぐ計算をやってきた、足郎2のシミュレーターだが、スクリプトからサーボを動かして、ロボットの形を作るところまでは、ほぼ出来上がった。このシミュレータに、COSMで書いたスクリプトを読み込ませて、動きを確認する程度には、実行させることができる。実際のロボットの動きにどれだけ近くなるかは、ロボットの方を組み上げてみないとわからないが、そちらは、業者に部品を作ってもらっているところなので、まず、シミュレータの動きを確認するしかない。
計算式が複雑なので、プログラムのバグを取るのに苦労した。
ただし、まだ、重心と着地した足の関係など、グローバルな座標で描いてはいない。ロボットの形をシミュレーションしているだけである。この動きを、重心との関係で、グローバルな座標に変換して表示するのはこれからの作業である。
このシミュレーターに足郎1を動かしたときのCOSMスクリプトを読み込ませて、動かしてみた結果が以下の動画である。

左が側面から見たもので、右側が正面から見たものであり、右足が緑色で、左足が青である。
足郎1には、膝がなかったので膝が動くスクリプトではないが、膝を動かすスクリプトも次に書いてみる。以下に、上記の動画の元になったスクリプトを記載しておく。

#######################################
# ashiro1-13.cosm
# 逆振り子二足歩行
# 2017年7月26日 作成
#######################################
%defspeed normal {
    interval:3
    steps:20
}
%defspeed fast {
    interval:3
    steps:10
}
#####################################
# かなり基本的なパラメータ
# いじるパラメータはここに置いておく
#####################################
# ただの逆振り子だと、40にすると倒れる
%param invpen_angle_right 35
%param invpen_angle_left 40
# 逆足を少し開いて、縮み加減にしないと倒れる
%param open_inv_right 15
%param open_inv_left 15
# 足を前に(わずかに)振り出す
%param step_forward 7
######################################
# 単純逆振り子
######################################
%defangles right_invpen {
#右に傾ける動きをする
    RightUpperRight:-$invpen_angle_right+$open_inv_right
    RightUpperLeft:+$invpen_angle_right-$open_inv_right
    LeftUpperRight:-$invpen_angle_left
    LeftUpperLeft:$invpen_angle_left
# 上の動きに合わせて、足も角度をつけて
    RightLowerRight:$invpen_angle_right
    RightLowerLeft:-$invpen_angle_right
    LeftLowerRight:$invpen_angle_left
    LeftLowerLeft:-$invpen_angle_left
}
%defangles left_invpen {
#左に傾ける動きをする
    RightUpperRight:$invpen_angle_right
    RightUpperLeft:-$invpen_angle_right
    LeftUpperRight:$invpen_angle_left-$open_inv_left
    LeftUpperLeft:-$invpen_angle_left+$open_inv_left
#
    RightLowerRight:-$invpen_angle_right
    RightLowerLeft:$invpen_angle_right
    LeftLowerRight:-$invpen_angle_left
    LeftLowerLeft:$invpen_angle_left
}
######################################
# 前へ
######################################
%defangles left_forward {
#左足を前に出す
    RightUpperFront:-$step_forward
    RightUpperBack:$step_forward
    LeftUpperFront:$step_forward
    LeftUpperBack:-$step_forward
}
%defangles right_forward {
#右足を前に出す
    RightUpperFront:$step_forward
    RightUpperBack:-$step_forward
    LeftUpperFront:-$step_forward
    LeftUpperBack:$step_forward
}
######################################
# 実行手続きの定義
# 即実行コマンドから最初に呼び出されるという意味で
# メインルーチン
# 初期歩行
######################################
%defexec walk {
    #stand:all
    exec:walk_sub,10
}
######################################
# サブルーチンの定義:通常歩行
######################################
%defexec walk_sub{
    speed:normal
    setAngle:right_invpen
    speed:fast
    setAngle:left_forward
    speed:normal
    setAngle:left_invpen
    speed:fast
    setAngle:right_forward
}
#################################
# 唯一の即実行プログラム:他は全て定義
# 全てはここから始まる
#################################
%exec walk 1
#################################