今回出展するお片付けロボットは以下のような動きでゴミ等を回収します．
↓
http://kyu-robo.sakura.ne.jp/movie/work/01_RC-DC/mech_movie_1.mp4

動画を見ていただくとわかるように，ロボット一台だけではゴミをすくい上げることが難しいと思われます．

なので少なくとも二台のロボットで協力してゴミをすくい上げることを考えます．

一台が壁となり，もう一台がゴミをすくい上げるように，二台でゴミを挟み込めると良さそうです．

そこで今回は，ゴミを挟み込むような経路計画を行いました．

大したロジックではないのですが，概要を説明します．

まず，ゴミから近いロボットを二台選択します(ここはざっくりで良いのでゴミとロボット間の直線距離を単純に比較)．
次にゴミが縦長か横長かで上下で挟み込むか左右から挟み込むかを決定します．
回収するまではゴミ自体には触れないので，ゴミは目的であると同時に障害物としても扱います．
ゴミから近い二台のみを制御し，ゴミを挟み込む位置まで移動します．
今回は位置精度が出るように，初動のみ，姿勢制御を優先させました．
残りの二台は有事に備え，姿勢だけ合わせてスタンバイします．

横長のゴミを挟み込む様子が↓です．
赤丸がゴミオブジェクト（画像認識から得られる点群），緑の丸が障害物（画像認識から得られる点群）です．
ロボットの経路が重なるとロボット同士がぶつかるリスクも増えるので選ばれた二台のロボット位置関係から，どっちのロボットがどちらから挟み込むも決めています．

また，縦長のゴミを挟み込む様子が↓です．

まだ，ゴミと障害物が近接しているときのエラー処理等が入っていませんが，概ね上手く動いてくれそうです．

あとはエラー処理とロボット同士が衝突しないようなロジックを入れ込めば，ひとまず良さそうです．