ロボットへの制御パケットの見直し

[HansRobo]

現状の制御パケット

https://github.com/ibis-ssl/crane/blob/develop/crane_sender/include/crane_sender/robot_packet.hpp

struct RobotCommand
{
  uint8_t HEADER;
  uint8_t CHECK;
  float VEL_LOCAL_SURGE;
  float VEL_LOCAL_SWAY;
  float VISION_GLOBAL_X;
  float VISION_GLOBAL_Y;
  float VISION_GLOBAL_THETA;
  float TARGET_GLOBAL_X;
  float TARGET_GLOBAL_Y;
  float BALL_GLOBAL_X;
  float BALL_GLOBAL_Y;
  float TARGET_GLOBAL_THETA;

  bool LOCAL_FEEDBACK_ENABLE;
  bool LOCAL_KEEPER_MODE_ENABLE;
  bool IS_ID_VISIBLE;
  bool STOP_FLAG;
  bool IS_DRIBBLER_UP;
  float KICK_POWER;
  float DRIBBLE_POWER;
  bool CHIP_ENABLE;
  operator RobotCommandSerialized() const;
};

struct RobotCommandSerialized
{
  enum class Address {
    //    HEADER,
    CHECK,
    VEL_LOCAL_SURGE_HIGH,
    VEL_LOCAL_SURGE_LOW,
    VEL_LOCAL_SWAY_HIGH,
    VEL_LOCAL_SWAY_LOW,
    VISION_GLOBAL_THETA_HIGH,
    VISION_GLOBAL_THETA_LOW,
    TARGET_GLOBAL_THETA_HIGH,
    TARGET_GLOBAL_THETA_LOW,
    KICK_POWER,
    DRIBBLE_POWER,
    LOCAL_FLAGS,
    BALL_GLOBAL_X_HIGH,
    BALL_GLOBAL_X_LOW,
    BALL_GLOBAL_Y_HIGH,
    BALL_GLOBAL_Y_LOW,
    VISION_GLOBAL_X_HIGH,
    VISION_GLOBAL_X_LOW,
    VISION_GLOBAL_Y_HIGH,
    VISION_GLOBAL_Y_LOW,
    TARGET_GLOBAL_X_HIGH,
    TARGET_GLOBAL_X_LOW,
    TARGET_GLOBAL_Y_HIGH,
    TARGET_GLOBAL_Y_LOW,
    SIZE
  };

LocalFlagの内容

  uint8_t local_flags = 0x00;
  local_flags |= (IS_ID_VISIBLE << 0);
  local_flags |= (LOCAL_KEEPER_MODE_ENABLE << 1);
  local_flags |= (STOP_FLAG << 2);
  local_flags |= (LOCAL_FEEDBACK_ENABLE << 3);
  local_flags |= (IS_DRIBBLER_UP << 4);

[HansRobo]

制御モードをいくつか用意したい

モード変数と自由記述スペースを確保して、モードによってAI側からロボットの動作モードを切り替えられるようにしたい。

uint8_t mode;
uint8_t mode_args[8];

自由記述スペース、8つじゃ足りないか？(floatなら4つ分くらい)

[KANAIHIROYUKI]

どういった制御モードがあるかについて考える。

1. 加速度優先　(目標速度への到達時間優先)
   今のモード。出せる限りの加速度でとにかく速く動くモード。
   加速度の異方性によって、進行角度がズレるのを許容する？
   (今の実装では)速度エラーはロボット側ではあまり補正していない。たぶん正確に目標速度が出ない
   長距離移動やキーパーはこれで良いと思う

2. 速度優先　(速度の再現性あり)
   進行角度がズレないようにする。ズレたら補正する。
   ロボットの向きによる加速度の異方性が出ないように、どの向きでも出せる加速度までしか出さない。
   (速度優先だけど加速度を最大限まで出す中間モードみたいなのはあっていいかも)
   実装的には、ロボット内で持っているグローバル座標を動かしてそれに追従する感じ？
   転がっているボールへのアプローチや、衝突時の速度がファウルに直結するアタック時にあるといいかも？
   ゴール前のブロッカーは、このモードで動かしたほうが、変に重なったり隙間ができたりするのを防げるかも。

3. (グローバル)座標優先
   指定された座標に対して高精度で追従するモード。
   ロボット側で勝手に目標座標までの速度プロファイルを作って追従する？
   パス待ちとかに使えるかも。ほかのモードで十分な精度が出たらいらないと思う。

[KANAIHIROYUKI]

･･･速度優先や座標優先でも、ロボット側odomは加速度を上げるとどうしても滑ってしまうので、誤差は出てしまう。
今の加速度優先モードをベースに、速度エラーをもうちょっとちゃんと補正するようにして、
最大加速度と、加速度の異方性を許容するかを与えれば十分な気もしてきた････

[HansRobo]

角速度の制御モードもここで変えられるといいかも
https://github.com/orgs/ibis-ssl/discussions/372#discussioncomment-9472909

[KANAIHIROYUKI]

制御モードを明確に分けるよりは、加速度の異方性を許容して加速度を最大化するか、進行方向のズレを補正するか、速度のオーバーシュートを許容するか、みたいな個々の制御機能を切り替えられる方が良い気がしてきた

[HansRobo]

ローカルカメラを使ったボール回り込みのためのモードも欲しい
https://github.com/orgs/ibis-ssl/discussions/414#discussioncomment-9727912

ボール回り込みは次のパスもセットで行われることがほとんどのはずだから、AI側から回り込み完了後の姿勢（主に角度）などの情報を自由記述スペースを使って渡したい。

[HansRobo]

軌道追従がうまく出来るようにロボットに情報を渡したい

今だと目標点を渡しているだけなのでもっと軌道追従しやすいように情報を渡したい。
軌道の接線情報を送るとか？

[KANAIHIROYUKI]

1. 起動追従専用ではないけど、起動追従性が良い制御モードを作っていい感じにする。
   この場合、制御モードの追加だけでいいかも。
   そもそも起動追従性が良い制御モードがなにか具体的に考えるほうが先かな･･･？

2. 起動追従専用のモードを作る。接線追従モードとか。
   どのみち軌道は逐次的に更新されていくので、軌道の断片だけを与えるので良い気がする。
   接線だけ与えるのはそれなりに妥当性はありそう。最適かはぜんぜんわからん････

[HansRobo]

接線（軌道の1次近似）で足りなければ2次近似、3次近似と近似する次数を上げて行けば良さそう。

[KANAIHIROYUKI]

どういうときに経路追従をしたいのか、どういった指令になりそうなのかがあんま理解できてないかも
なんか具体例あると嬉しい

[HansRobo]

一番はボールに当たらず、ボールの周りを動き回るときかな
ボールは軽く当たるだけで動いちゃうのでちゃんと起動追従してくれないとゆっくり動くしかなくなる。
強いチーム見てるとセットプレーとかの素早さでかなり優位に立つ場面も結構ある

[KANAIHIROYUKI]

速度指令値をグローバル座標系にする?

どっかのタイミングでこの話が出た気がするけど、結局、座標系が違うだけで同じ値になるので意味ない気がしてきた。
visionが無い動作チェック時などはローカル座標系のほうが便利な気もするので、今のままでよさそう

[HansRobo]

フラグ用のビットが余ってるのでローカル・グローバル切り替えフラグを作っても良いかも

[KANAIHIROYUKI]

思い出した、ロボットが旋回していると、Visionの遅延ぶんズレるという話だった。
グローバル座標系で指令を送るメリットはあるはず。とはいえ、レイテンシ情報はどのみちodomのために使うから、ロボット側で補正かけるのでもよさそうだし、すでに実装していたように、AI側で予め補正かけるのでもそれなりに効くはず

[HansRobo]

現状、メインで使っている位置指令がグローバル座標系なので速度でわざわざローカル座標系にする意味も無さそう

[KANAIHIROYUKI]

ヨー方向についてもう少し情報を増やす

今は角度指定のみだが、速度や加速度や追従モードなど追加する

角速度指定　(レイテンシチェック)

今はロボット側でレイテンシチェックモードを作り、角速度一定で回転させているが、開始が面倒なので普通に制御コマンドで角速度一定で回転させられたほうが良いかも。
まあ、指令角度を一定で変えるだけでいい気もする

角度追従の優先度切り替え (角度優先 or 移動優先)

シュートしたい場合などはロボットの向きは指令値どおりであってほしい。
パスカットのような動きの場合、ロボットの向きはなんでもいいので、加速を優先したい。

角加速度(?)変更

ボールプレースメントなど、機敏さよりも滑らかさが重視されるときに角加速度を下げたい。

ヨー角度ゲイン(各加速度)を与えて、滑らかさや追従優先度を切り替えられると良いかも。
角速度指定はなくてもいいかな････

[KANAIHIROYUKI]

開発方針

とりあえず考えてみた

コマンド拡張

使い方はともかく、とりあえずデータ量だけ拡張しておく？

制御モード(ロボット側)

最終的にどうするかはともかく、小機能単位で実装する
加速度下げる代わりにロボットの角度による異方性がないモードを作る
速度制御をもう少し賢く(できるように)する
座標制御モードは必要かどうかわからん、けど仮で作っても良いかも

制御モード(AI側)

状況によって、ロボット側がなにを優先して動くべきかを考えられればいいかも？
具体的にどのモードで動くべきかまではまだ決めきらなくてよさそう

[KANAIHIROYUKI]

ibis-ssl/G474_Orion_main@bc41048
てきとうに拡張してみる

[HansRobo]

ローカルカメラの制御をONにしてもカメラの画角にボールが入ってないと出来ないので、ボールが見えない時のフォールバック用に目標速度送っても良いかも

[KANAIHIROYUKI]

https://github.com/ibis-ssl/Orion_CM4/tree/new_ai_cmd
CM4側をアプデ
forward_ai_cmd_v2.cpp
30byte→64byteに拡張 + 整理 + baudrateを引数で指定できるように変更

[HansRobo]

↑2 いや、目標速度じゃなくてビジョンのボール座標送ってあとはローカルで画角に入るように制御すれば良いことに気づいた

[KANAIHIROYUKI]

AI視点ではローカルカメラ使えるはずだが、実際にロボット側でボールを認識できない条件
･見えているけど認識できない
･ほかのロボットに隠されている
･画角から外れている

フォールバック時の動作について
･再びローカルカメラで認識できるのを期待して、動きの連続性を重視する
･ローカルカメラが使えないものとして他の動作に入らせる(AI側からの指令に戻る)か

画角(ボールに対するヨー角度)制御はローカルでやるとして、
フォールバック動作は、一瞬なら連続性を重視(直前の動作を維持)、それ以上ならAI側動作に戻るのでどうだろう

[KANAIHIROYUKI]

ロボットの制御以外のデータ

追加でなにかあるといいかもしれない

使いそうなデータ
ロボット→AIのフィードバックデータの切り替え
動作(デバッグ)モードの切り替え

[KANAIHIROYUKI]

ロボットに与えるパラメーターの追加

速度指令モードで加速度も与えるようにする？

[HansRobo]

追加した。もうすぐマージされるはず
#550

[KANAIHIROYUKI]

追加の神

[HansRobo]

@KANAIHIROYUKI
テストも通ったのでコピーして使ってけろ
https://github.com/ibis-ssl/crane/blob/develop/crane_sender/include/crane_sender/robot_packet.h

[KANAIHIROYUKI]

ｸﾞﾜｯｸﾞﾜｯｸﾞﾜｯ