見出し画像

【フィジカルAI】複数ワークの順次ピッキング — カメラで「全部見つけて」1個ずつ順番に並び替える

FCTX

Physical AIを、一歩ずつ自分の手で作って学ぶシリーズ。 第3回です。

  • 第1回：座標既知＋自作IKでピック＆プレース

  • 第2回：RGB-Dカメラ＋色検出で「1個」を見つけてつかむ

  • 第3回（今回）：複数個をまとめて見つけて、1個ずつ順番に並べ替える ← いまここ

  • 第4回（予定）：IKを強化学習ポリシーに置き換える

コードはGitHubで公開しています。1テーマ＝1ファイル、機械学習もOpenCVも使わず、numpyだけで読み切れる分量にしてあります。

リポジトリ：Open_Physical_AI/03_multi_picking/multi_picking.py

今回やること

#02では、赤いキューブ1個をトップダウンカメラで見つけて拾いました。今回はこれを複数個に拡張します。バラバラに置かれた3個のワークをカメラで一度に全部見つけて、1個ずつ順番に一列のスロットへ並べ替える。工場の「整列作業」のミニチュア版です。

ロボットを動かす土台——ヤコビアンIK、8フェーズの状態機械——は#01・#02とまったく同じです。今回増えたのは3つだけ。

追加要素何をするかブロブ分割赤マスクを連結成分に分けて、ワーク1個ずつの重心を取るタスクキュー検出した全ワークを処理順に並べ、1個終わったら次へ標準アセットのワーク自作キューブをやめ、Isaac Sim標準アセット（YCBのスポンジブロック）を使う

「1個できるなら3個は同じでしょ？」と思うかもしれません。半分正解です。ロボットの動きは本当に同じ。でも「画像の中の赤い塊を、どうやって“1個ずつ”に切り分けるか」と「どの順番で手を付けるか」という、複数ならではの問題が新しく生まれます。ここが今回の本題です。

全体の流れ

サイクル頭 : ホーム姿勢へ戻して視界を確保 → 全ワークを検出 → 処理順に並べてキューへ

ワーク 1 : 上空→下降→把持→持上→搬送→設置→解放→退避（8フェーズ）

ワーク 2 : 同じ8フェーズ（★検出はやり直さない）

ワーク 3 : 同じ8フェーズ

全ワーク完了: ワーク側とスロット側を入れ替えて次サイクルへ（往復デモ）

ポイントは、検出はサイクルの頭で1回だけという設計です。

1個置くたびに撮り直す設計も考えられますが、やってみると分かる落とし穴があります。腕が動いた後に撮り直すと、すでに並べ終えたワークも「未処理のワーク」として写ってしまうのです。どれが済んでいてどれが未処理か、画像だけからは区別できません。最初に全部数えてキューに固定してしまえば、この問題は最初から存在しない。「賢く毎回見る」より「最初に1回だけちゃんと見る」方が、素直で安定します。

本題1：赤い塊を「1個ずつ」に切り分ける

#02の検出はこうでした。

  1. RGB画像から「赤いピクセル」をしきい値で抜く（マスク）

  2. マスク全体の重心を1つ求める

  3. 深度を読んで世界座標へ逆投影する

ワークが1個なら、これで十分。でも3個あると、手順2が壊れます。マスク全体の重心は3個の真ん中の何もない空間を指してしまう。ロボットは律儀にそこへ降りていき、空気をつかみます。

必要なのは、マスクを連結成分（つながった塊＝ブロブ）ごとに分けて、それぞれの重心を取ることです。OpenCVならconnectedComponents一発ですが、このシリーズの方針は「中身を見せる」。numpyだけで書きます。

ラベル伝播——「大きい番号に染まっていく」

やり方はとても素直です。

  1. マスク内の全画素に「自分だけのユニークな番号」を配る

  2. 上下左右の隣の最大値を取り込む、を変化がなくなるまで繰り返す

def label_blobs(mask, max_iter=1024):

labels = np.full(mask.shape, -1, dtype=np.int64) # 背景は -1

labels[mask] = np.arange(int(mask.sum())) # 全画素に固有番号

for _ in range(max_iter):

# 外周を -1 で囲ってから4方向へずらす（境界の場合分けを消すため）

padded = np.full((h + 2, w + 2), -1, dtype=np.int64)

padded[1:-1, 1:-1] = labels

neighbor_max = np.maximum.reduce([

padded[:-2, 1:-1], # 上

padded[2:, 1:-1], # 下

padded[1:-1, :-2], # 左

padded[1:-1, 2:], # 右

])

updated = np.where(mask, np.maximum(labels, neighbor_max), -1)

if np.array_equal(updated, labels):

return labels # 変化なし = 収束

labels = updated

つながっている画素同士は、反復のたびに「隣の大きい番号」を取り込んでいくので、最終的にその塊で最大の番号にみんな染まります。染まり終われば「1つの塊＝1つのラベル」。別の塊とは一度もつながらないので、番号が混ざることもありません。floodfill（バケツ塗り）を全ブロブ同時にやっている、と思えばOKです。

番号は1回の反復で1画素ずつしか広がらないので、反復回数はブロブの直径ぶん必要です。1280×720の全画面で回すと無駄が多いため、赤画素を囲む矩形だけを切り出してから回します。矩形の外は全部背景なので、切り出しても連結関係は変わりません。この工夫込みで、手元の実測は1回あたり約0.2秒。サイクル頭に1回だけなので、体感ではまったく気になりません。

ブロブごとに重心→逆投影

ラベルが付いたら、あとは#02と同じことをブロブの数だけ繰り返します。

  • ブロブごとに重心 (u, v) を中央値で取る（縁のにじみに強い）

  • 深度 d もブロブ内の中央値

  • 自前ピンホールモデルで (u, v, d) → 世界座標へ逆投影

  • 小さすぎる塊（min_pixels未満）はノイズとして捨て、大きい順に採用

実行ログがこちら。3個がきれいに分かれています。

blob: px=3671 u=702.0 v=621.0 d=1.399 -> world=(0.679, -0.159, 0.051)

blob: px=3652 u=681.0 v=264.0 d=1.399 -> world=(0.659, 0.182, 0.051)

blob: px=3612 u=765.0 v=432.0 d=1.399 -> world=(0.739, 0.021, 0.051)

detected 3 work(s) (expected 3)

ワークの真の位置は (0.68, -0.16)、(0.66, 0.18)、(0.74, 0.02)。誤差は1〜2mmです。

本題2：どの順番で手を付けるか——タスクキュー

複数あるとき「どれから拾うか」は、地味に見えてロボットの見せ場です。今回は検出結果を処理順に並べ替えて、キュー（待ち行列）に積みます。

def _sort_works(self, works):

order = self.cfg.control.pick_order

if order == "near": # ロボット基準に近い順（既定）

return sorted(works, key=lambda p: float(np.hypot(p[0], p[1])))

if order == "far": # 遠い順

return sorted(works, key=lambda p: -float(np.hypot(p[0], p[1])))

if order == "y": # Y座標の昇順（見た目で追いやすい）

return sorted(works, key=lambda p: float(p[1]))

return list(works) # 検出順（ブロブの大きい順）

既定は「近い順」。手前から片付けるので腕の移動が小さくて済みます。設定1行で「遠い順」「Y座標順」に切り替えられるので、動きの違いを見比べてみてください。

状態機械側の変更は驚くほど小さくて、最終フェーズ（退避）が終わったときの分岐に3行足しただけです。

# 最終フェーズ完了 = 1ワーク分の処理が終了

self.work_idx += 1

if self.work_idx < len(self.queue): # キューに次があれば

self.phase = Phase.APPROACH_ABOVE # 検出せず先頭フェーズへ戻る

return

# キューが空 = 1サイクル終了 → ワーク側とスロット側を入れ替えて次サイクル

i番目に拾ったワークはi番目のスロットへ。ワークはすべて同じ形なので「どれがどこへ行くか」は気にしません（区別が必要になったら、それは#02でやった「検出値とIDの対応付け」という新しいテーマになります）。

本題3：ワークをIsaac Sim標準アセットに——そして2つの罠

これまでのワークはUsdGeom.Cubeで自作した赤い箱でした。今回はIsaac Sim標準アセットのYCBスポンジブロック（061_foam_brick.usd）に差し替えます。YCBはロボット研究の定番オブジェクトセットで、実在する物体の3Dスキャンです。

assets_root = get_assets_root_path()          # アセットサーバーのURL

url = assets_root + "/Isaac/Props/YCB/Axis_Aligned/061_foam_brick.usd"

stage_utils.add_reference_to_stage(url, "/World/Work_0")

実測サイズは77.9 × 51.2 × 52.6mm。短辺51.2mmを、Robotiq 2F-85（開き85mm）でつかみます。ここで罠が2つありました。

罠1：参照しただけでは「見えるだけの飾り」

自作キューブのときはRigidBodyAPIやCollisionAPIを自分で付けていたので意識していましたが、標準アセットには物理が何も付いていません。RigidBodyもColliderも質量もなし。参照しただけだと、床をすり抜けて落ちるか、逆にその場に浮いたまま、グリッパーがすり抜けます。

for descendant in Usd.PrimRange(prim):        # アセット内の全メッシュに

if descendant.IsA(UsdGeom.Mesh):

UsdPhysics.CollisionAPI.Apply(descendant) # 衝突形状

UsdPhysics.MeshCollisionAPI.Apply(descendant) \

.CreateApproximationAttr(UsdPhysics.Tokens.convexHull) # 凸包近似

UsdPhysics.RigidBodyAPI.Apply(prim) # 剛体化

UsdPhysics.MassAPI.Apply(prim).CreateMassAttr(0.05) # 質量

衝突形状は凸包（convexHull）で近似します。箱型のワークなら凸包＝ほぼ形そのもので、計算も軽い。

罠2：アセット選びは「上から見た色」で決まる

今回の検出は「上から見て赤いか」がすべてです。それっぽい候補を4つ、実際のステージ・実際のカメラで撮って測ってみました。

アセット上面の色（実測RGB）判定Blocks/basic_block（47mm角）(182, 182, 182) 白✗ 赤検出不可Blocks/nvidia_cube（65mm角）(26, 122, 19) 緑✗Rubiks_Cube（72mm角）面ごとに色が違う✗ 不安定YCB/061_foam_brick(130, 55, 52) 赤◎ しきい値変更なしで検出

見た目のカタログだけで選ぶと失敗します。basic_blockは名前的に一番それっぽいのに白。foam_brickだけが、#02の赤しきい値（「RがG・Bより55以上大きい」）を1文字も変えずに通りました。

ちなみにこの色測定でも一回転びました。最初、何もないステージにワークだけ置いて撮ったら全ピクセル真っ黒（ライトがない）。ライトを自作して撮り直したら今度は全部白飛び（強すぎ）。結局「本番と同じステージ・同じ照明で測る」のが唯一意味のある測定でした。カメラを使う実験の鉄則です。

動かした結果

3個のワークを近い順に拾って、一列のスロット（Y方向に0.14m間隔）へ並べます。

detected 3 work(s) → queue: (0.659, 0.182) → (0.679, -0.159) → (0.739, 0.021)

work 1/3 placed

work 2/3 placed

work 3/3 placed

cycle 1 done -> starting next cycle

並べ終えたかどうかは、次のサイクルの検出結果が答え合わせになります。サイクル2の頭でカメラが見たワーク位置がこちら。

検出: (0.499, 0.060)  (0.499, 0.199)  (0.499, 0.339)

目標: (0.500, 0.060) (0.500, 0.200) (0.500, 0.340)

設置誤差は1〜2mm。3個ともきれいに一列です。サイクル2ではスロット側を拾って元のバラバラ配置へ戻し、以降ずっと往復し続けます。

おまけのハマりどころ：printしても何も出ない

実は今回いちばん時間を食ったのは、検出でもIKでもなくログが1行も出ないことでした。

Pythonのlogging.basicConfigは「rootロガーにハンドラがまだ無いときだけ」設定してくれる関数です。ところがIsaac Sim（Kit）は起動時に自分のハンドラをrootへ付けてしまうので、スクリプト側のbasicConfigは黙って何もしません。しかもKitのハンドラはINFOを表示しない。結果、log.infoが全部闇に消えます。

対策は、自分のロガーに直接ハンドラを付けて、rootへ流さないこと。

log = logging.getLogger("multi_picking")

log.setLevel(logging.INFO)

handler = logging.StreamHandler(sys.stdout)

handler.setFormatter(logging.Formatter("[MULTI_PICKING] %(message)s"))

log.addHandler(handler)

log.propagate = False # Kit のハンドラへ流さない

Isaac Simに限らず、「大きなフレームワークの中で自分のスクリプトを動かす」ときの定番の罠です。エラーは出ないのに出力だけが消えるので、知らないとかなり悩みます。

設定を変えて遊ぶ

いつも通り、パラメータはdataclassにまとめてあります。

設定意味SceneConfig.work_positionsワークの初期位置。要素を増やせばワークもスロットも自動で増えるSceneConfig.slot_origin / slot_pitch並べ先の起点と間隔ControlConfig.pick_order処理順（near / far / y / 検出順）ControlConfig.loopFalseで1サイクルで停止CameraConfig.min_pixels / max_blobsノイズ除去と採用ブロブ数の上限

おすすめの実験は2つ。

  • ワークを4個・5個に増やす：work_positionsに座標を足すだけ。スロットが画角からはみ出したらカメラ位置（CameraConfig.position）を調整

  • 2個をわざと接触させて置く：ブロブが1つにつながって「2個が1個」に見えます。しきい値処理の限界を体感できる、いちばん教育的な失敗です（分離したくなったら距離変換＋watershedの世界へどうぞ）

まとめと次回

今回増えたものを振り返ると：

  • ラベル伝播：numpy10行で書ける連結成分分割。「隣の最大値を取り込み続けると塊ごとに染まる」

  • タスクキュー：状態機械の外側にもう1周ループを重ねるだけ。追加3行

  • 標準アセット：物理は自分で付ける。アセット選びは「上から見た色」を実測で確認

#01から数えると、「座標を教わって動く」→「カメラで1個見つける」→「複数見つけて順に処理する」まで来ました。ロボットの動かし方そのもの（IK＋状態機械）は第1回から一度も変えていません。知覚とタスク管理だけが進化しているのがこのシリーズの設計です。

次回#04は、そのIKをいよいよ強化学習ポリシーに置き換えます。「自分で書いたIK」があるからこそ、学習に置き換えたとき何が変わるかを比べられるはずです。

関連記事

Isaac Simのインストール手順

USDファイル準備の記事

第1回目の記事

第2回目の記事

フィジカルAI・工場向けAI開発のご相談

私は製造業向けのAIシステム開発を行っています。
こんなお悩みはありませんか？

* 外観検査を自動化したい
* 作業監視をAI化したい
* PPE（保護具）着用チェックを導入したい
* ロボットアームへAIを組み込みたい
* ROS2・Isaac Simを活用したい
* フィジカルAIのPoCを進めたい
* 社内向けAIツールを開発したい
* Isaac simを使ってロボット操作したい

要件整理からPoC、実装まで対応可能です。
まずはお気軽にご相談ください。

お問い合わせ
https://forms.gle/qTxgVfBF622eiBWA6

技術ポートフォリオ
GitHub

Hugging Face

note

いいなと思ったら応援しよう！

FCTX 無料で気軽に使えるアプリを個人で開発しています！ いただいたチップは、アプリの改善や開発環境の維持に大切に使わせていただきます。 応援していただけると、とても励みになります！