実機と同じ校正で動かすDobot Magicianシミュレーター 閉ループSim2Realの設計

「緑の5のカードを中央に置いて」と日本語で指示すると、VLMが対象を選び、2台のDobot Magicianのどちらを使うか決め、動作後にカメラで結果を確かめる。この一連の処理を、実機を動かさずに最後まで検証できるシミュレーターを作りました。
目標は、見栄えのよい3Dデモを作ることではありません。実機用のROS 2ノード、認識処理、planner、executorを変えず、ロボットとカメラだけを仮想環境へ差し替えることです。特にカメラは設計上の位置へ置くのではなく、実機の校正ファイルから位置・姿勢・レンズ歪みを逆算しました。
このシミュレーターが対象にしている、音声・VLM・物体認識・2台のロボットアームをつないだ実機システムの全体像は、次の記事で紹介しています。
この構成により、実機用のcamera.yamlとhomography.yamlをシミュレーションでもそのまま使用できます。12シーン・88物体で認識を評価したところ、ブロック、レゴ、ボール、UNOカードの位置誤差は平均0.4〜0.8mmでした。一方、形状や質感を再現しきれていない取っ手付きレゴでは、種類の認識に失敗しています。Sim2Realの差はゼロではありませんが、どこまで合い、どこが合わないかを数値で見られる環境になりました。
1. Sim2Realで合わせたのは見た目だけではない
今回のSim2Realでは、次の4層をそろえました。
| 層 | 実機との共通化 | 狙い |
|---|---|---|
| 制御インターフェース | 実機用magician_ros2を使用 | 上位ノードを実機とシミュレーションで分岐させない |
| ロボット状態 | 実機と同じ姿勢・関節角・道具操作のAPI | plannerとexecutorを同じ条件で動かす |
| カメラ幾何 | 実機の内部パラメータ、歪み、ホモグラフィ | 同じ画素を同じマット座標へ変換する |
| 検証ループ | 動作後に再撮影し、失敗時は探し直す | 成功する前提の再生ではなく、失敗回復まで試す |
一般にシミュレーターというと、物理エンジン内でロボットと物体を動かす部分へ目が向きます。しかし今回のシステムで重要だったのは、実機のソフトウェア境界を保つことと、認識ノードへ渡る画像を実機の幾何に合わせることでした。

設計値を使った初期のカメラです。マットの奥の端から高さ650mm、傾き18度、歪みなし、画像を180度回転する前提でした。

実機の校正ファイルから再現したカメラです。位置は約(307.0, 79.8, 556.3)mm、鉛直から14.7度で、レンズ歪みと主点のずれも反映しています。2枚はどちらもシミュレーション画像ですが、後者は実機の認識処理へ入力する条件に合わせています。
2. カメラとロボットだけを仮想環境へ差し替える
システム全体は、日本語の指示を受けるUI、VLMを使うplanner、物体認識、executor、2台のロボットアームで構成しています。シミュレーション時に置き換えるのは、図の右側にあるカメラとロボットです。
実行環境はUbuntu 22.04、ROS 2 Humble、MuJoCo 3、RTX A2000 8GBです。認識にはOpenCVによるHSVと形状の当てはめ、および実機画像で追加学習したRF-DETR-Segを用途に応じて使います。計画側ではOllama上の量子化したQwen 3.5 9Bと、到達範囲や干渉を確認するルールベースのplannerを組み合わせています。
3. 実機ドライバの手前でシリアル通信だけを差し替える
magician_ros2は、dobot_driver.interface.Interfaceを通じてロボットのファームウェアとシリアル通信します。環境変数MAGICIAN_SIM=1のときだけ、このクラスをFakeInterfaceへ差し替えました。
class FakeInterface:
def __init__(self, port, timeout=10.0):
mgr = SimManager(address=SOCK, authkey=AUTHKEY)
mgr.connect()
self._robot = mgr.get_robot(port)
def __getattr__(self, name):
return getattr(self._robot, name)
FakeInterfaceは、別プロセスのsim_serverにあるSimRobotへ呼び出しを転送する薄い代理です。メソッド名を実機のInterfaceとそろえたため、ドライバ側のROS 2ノードをシミュレーション専用に書き換える必要がありません。
2台の腕の状態をsim_serverへ集めることで、動作サーバー、状態配信、グリッパーや吸盤のノードが同じ腕の状態を参照します。腕同士の干渉と、物体をつかむ・運ぶ・放す処理もここで管理します。
ROS側のPython環境はnumpy 1.21、MuJoCoや検出器を動かすGPU側はnumpy 2系です。依存関係を無理に混ぜず、画像は一時ファイルからos.replaceでアトミックに差し替えるか、JSONヘッダーと生バイトのソケットで渡します。
4. 物理演算より、再現可能な運動学と失敗注入を優先した
MuJoCoは使っていますが、物理時間を進めるmj_stepは呼んでいません。担当させたのは、関節角からリンク姿勢を求める運動学、EGLによるオフスクリーン描画、深度画像と物体マスクの生成です。
Magicianはステッピングモーターの位置制御で、今回の検証では力や接触を精密に再現するより、指令した軌道と上位ソフトウェアの反応を再現する方が重要でした。SimRobotは実機と同じリンク長を使い、100HzのsmoothstepでMOVJとMOVLを補間します。速度と加速度の比率も反映し、実機と同じ40%の設定で動かします。
物体はマット座標上の位置、向き、高さを持ちます。グリッパーを閉じたときや吸盤をONにしたとき、先端が物体上面の範囲と高さに入っていれば把持状態へ移します。放すと真下で最も高い面へ落とし、積み上げを表現します。
接触物理を簡略化した代わりに、つかみ損ね、運搬中の落下、配置後の滑りを確率で発生させられます。これは見た目のリアリティよりも、executorの結果確認と立て直しを繰り返し試すための設計です。
5. 実機の校正ファイルからカメラ姿勢を逆算する
実機では、次の2つの校正ファイルを使っています。
camera.yaml: 内部パラメータとレンズ歪み係数homography.yaml: 歪み補正後の画素からマット座標へのホモグラフィ
マット面は高さ0の平面です。そこで、内部パラメータとホモグラフィの逆行列から、カメラの回転と並進を分解しました。回転軸は外積で補い、SVDで最も近い正規直交行列へ直します。カメラ位置は、得られた回転と並進から計算できます。
逆算したカメラ位置は(307.0, 79.8, 556.3)mm、傾きは鉛直から14.7度でした。別の校正ツールが出していた(306.4, 80.3, 555.4)mm、14.7度と一致しています。
座標系には注意が必要でした。マット座標は、観客から見てxが右、yが手前、zが上の左手系です。計算内部ではzを下向きにした右手系を使い、MuJoCoの世界座標へ移すときにyとzを反転しています。
6. レンズ歪みと主点のずれは画像変換で再現する
MuJoCoのカメラは、主点が画像中央にある歪みなしのピンホールカメラです。実機カメラの主点は(954, 591)で、1920×1080画像の中央から51px下へずれています。レンズ歪みもあるため、MuJoCoのカメラ設定だけでは一致しません。
そこで、少し大きな2338×1476のピンホール画像を描き、事前計算した対応表を使ってcv2.remapで1920×1080へ変換しました。
歪みの逆算では、画像の隅で不動点反復が発散しました。右上では正規化座標が0.87 → 1.96 → 1.02と振動します。各画素について、順方向へ写し戻したときの誤差が最小だった推定値を保存するようにして、隅でも破綻しない対応表にしました。
もう一つの問題は、ホモグラフィから得た回転行列をSVDで直交化すると、マット面で最大2.3pxずれることでした。実測ホモグラフィには校正誤差も含まれるため、数学的にきれいな回転へ直すだけでは画素が一致しません。そこで、モデルが作るマット面の射影と校正値との差を画像空間の射影変換としてremapへ組み込みました。既知点を描いて測ったずれは0.4px以下です。
7. 作業台と照明も数値で実機へ近づける
カメラだけでなく、ArUcoマーカー、UNOカード、物体、ロボット台、カメラ支柱、机の色も実物に合わせました。
| 項目 | 設計値を使った初期版 | 実機校正へ合わせた版 |
|---|---|---|
| ArUco | マット外、40mm | マット上の実測位置、35mm |
| カード | ひらがなカード | UNOカード |
| 物体 | ブロック、レゴ、ボール | 取っ手付きレゴも追加 |
| ロボット台 | 回転軸が四角の中心 | 四角の中心は回転軸の17.5mm前 |
| 背景 | 灰色の机 | 実機に近い暗い机 |
台の位置ずれを監視する赤いシールは、基準画素から伸ばした視線と台の上面との交点へ配置しました。最初は2〜3pxのずれが残り、監視処理が右の台を3.4mmずれていると判定しました。原因は、マットの厚さ2mmを座標へ加えていなかったことです。修正後の判定値は0.54mmと0.86mmで、警告基準の1mm以内に収まりました。
照明は、何も置いていないマットの同じ位置で画素値の中央値を比較しました。
| 場所 | 実機 | 初期版 | 調整後 |
|---|---|---|---|
| 中央 | 133 | 183 | 138 |
| 左手前 | 109 | 185 | 113 |
| 右中ほど | 144 | 175 | 150 |
初期版の真上からの平行光3灯を、右上のスポットライト1灯と環境光へ変更しました。最後にWebカメラ相当のガウスぼけσ=0.8pxとノイズσ=2.5を加えています。
8. 同じシーンから深度と学習用マスクも作る
MuJoCoのRendererを切り替えると、RGB画像だけでなく、深度とgeom単位のセグメンテーション画像も取得できます。
セグメンテーション画像では、画素ごとのgeom番号を物体へ対応させ、隠れていない領域だけをマスクにします。照明、ノイズ、ぼけ、JPEG圧縮をランダム化し、RF-DETR-Segの初期学習データへ利用しました。
深度画像は単眼深度推定の評価用の正解値になります。チェッカーボードを仮想作業台へ置けば、既知の歪みを加えた画像からカメラ校正手順そのものも検証できます。Sim2Real用のシミュレーターを、認識モデルと校正ツールのテストデータ生成器としても使える構成です。
9. 閉じたループで認識・動作・再確認まで回す
物体や腕の状態が変わると、MuJoCoカメラが画像を描き直し、sim_serverが/camera/image_rawへ配信します。executorは実機と同じ経路で画像を取得し、動作後に対象が目的地へ移ったか確認します。
腕だけが動いた場合はqposを更新してmj_forwardを呼びます。物体の種類や数が変わった場合だけMJCFを組み立て直します。モデル再コンパイルは23〜97ms、2338×1476の描画は約25ms、歪み変換とノイズ付加は約2msでした。状態は0.2秒ごとに確認し、最大で約5回/秒描き直します。
10. 実機用の認識処理をそのまま通した結果
実機カメラを再現した12シーン、合計88物体を、実機のcamera.yamlとhomography.yamlを変更せずに認識させました。この評価では検出器を使わず、HSV、形状の当てはめ、カメラ幾何を使用しています。
| 種類 | 検出 / 種類正解 / 全体 | 位置誤差 平均 / 最大 |
|---|---|---|
| ブロック | 21 / 21 / 21 | 0.5 / 0.7mm |
| レゴ2×4 | 17 / 17 / 17 | 0.5 / 0.6mm |
| レゴ1×6 | 6 / 6 / 6 | 0.4 / 0.5mm |
| ボール | 17 / 17 / 17 | 0.8 / 1.4mm |
| UNOカード | 20 / 20 / 21 | 0.5 / 0.7mm |
| 取っ手付きレゴ | 6 / 0 / 6 | 8.7 / 11.0mm |
積み上げの段数は44/44、段ごとの色は52/52で正解しました。
UNOカードは、最初に白い楕円を大きく描きすぎたため、色の面が分断され、20枚すべてをotherまたはlegoと誤認識しました。実物の写真と比べ、楕円を色面の幅の約6割へ縮めると20/20で認識できました。人には小さな見た目の差でも、色領域を使う認識処理には決定的な差になります。
取っ手付きレゴは未解決です。実機画像で学習したRF-DETR-Segを使っても、シミュレーションではブロックと誤認識します。ホースの輪の質感と太さ、灰色のテクニックブロックの見え方が実物と合っていません。良い結果だけでなく、この失敗が可視化されたこともシミュレーターの役割だと考えています。
11. Sim2Realで残っているギャップ
- 物体は接触物理で転がったり倒れたりせず、落下や滑りをルールと確率で模擬している
- J1の脱調、台の滑り、ホーミング後の0度ずれなど、実機固有の故障モードは未実装
- 実機の部屋に合わせた照明は1条件のみで、窓光や時間帯の変化は評価条件として別に扱っている
- 取っ手付きレゴの形状と材質は、実機画像で学習した検出器へ転移できる精度に達していない
この構成は、シミュレーションだけで実機の成功率を保証するものではありません。実機へ移した後も、認識誤差、把持の成否、干渉、故障モードの確認が必要です。シミュレーションは実機検証を置き換えるのではなく、実機でしか見つからない問題へ時間を使えるよう、ソフトウェア境界と基本動作を事前に繰り返し検証するために使います。
12. Sim2Realで効いた設計判断
実機とシミュレーションを別システムとして育てるのではなく、シリアル通信と画像生成の境界だけを差し替えました。上位のROS 2ノードと校正ファイルを共通化できたことで、シミュレーション専用コードで成功しているだけ、という状態を避けられます。
もう一つ効いたのは、「実機に近い」を画素位置、明るさ、認識位置誤差で測ったことです。マットの厚さ2mm、カード模様の比率、主点の51pxのずれといった差は、見た目の印象だけでは優先順位を付けにくいものです。認識処理を実際に通し、差が出た場所を直すことで、Sim2Realの作業を具体的な検証へ変えられました。
- MuJoCoの物理シミュレーションを使わないのはなぜですか?
- 今回の主目的は、Magicianの力学を精密に再現することではなく、実機と同じROS 2経路、カメラ認識、計画、実行後の確認を繰り返し試すことです。腕は運動学で動かし、把持や落下は再現可能なルールと失敗注入で扱う方が、executorの検証に向いていました。
- 実機とシミュレーションで同じカメラ校正ファイルを使えますか?
- はい。実機の内部パラメータ、歪み係数、ホモグラフィから仮想カメラを作っているため、認識側は実機用の
camera.yamlとhomography.yamlをそのまま読みます。 - シミュレーションで動けば、実機でもそのまま成功しますか?
- 成功を保証するものではありません。脱調、台の滑り、把持力、物体の材質、照明変化など、実機固有の差は残ります。シミュレーターは上位ソフトウェアと基本動作を先に検証し、実機テストをより重要な差へ集中させるためのものです。
- 合成画像は物体検出の学習にも使えますか?
- 同じシーンからRGB、深度、物体マスクを生成できるため、初期学習データには利用できます。ただし、取っ手付きレゴのように形状や質感の差が大きい対象では、実機画像で学習した検出器へうまく転移しません。実画像での評価と追加学習は必要です。
- 2台のロボットアームの衝突はどのように扱いますか?
- MuJoCoの接触判定ではなく、腕を線分で近似し、線分間の最短距離を
sim_serverで監視します。executor側でも共有ゾーンをロックしますが、あらゆる姿勢での完全な衝突回避を保証するものではありません。






