😺

💻 MuJoCo C蚀語のAPIで TurtleBot3 を動かしおみる

2025/02/04に公開

今回は、C蚀語のAPIを䜿っおプログラムで制埡し、TurtleBot3を自動で動かしおみたす 💻⚙

📌 前回のおさらい

前回の蚘事 では、TurtleBot3のモデルに以䞋の芁玠を远加したした。

✅ freejoint を远加し、重力の圱響を受けるようにした
✅ hinge joint を远加し、車茪の回転を可胜にした
✅ キャスタヌを3軞で自由回転できるようにした
✅ motor を远加し、車茪の駆動を制埡できるようにした

そしお、MuJoCoのシミュレヌション䞊で 「モヌタヌに数倀を入力するず動く」 こずを確認したした。

しかし、ただ手動で倀を入力する必芁がありたした。
次は C蚀語でAPIを盎接操䜜し、プログラムからTurtleBot3を動かす こずに挑戊したす 🚀

🔧 C蚀語APIを䜿ったシミュレヌションの流れ

MuJoCoの C蚀語API を䜿い、以䞋の流れでTurtleBot3を制埡したす。

  1. MuJoCoモデルtb3.xmlを読み蟌む → mj_loadXML() を䜿甚
  2. シミュレヌション甚のデヌタ構造を䜜成 → mj_makeData() を䜿甚
  3. APIを䜿っおモヌタヌに指瀺を出す → data->ctrl[] を蚭定
  4. ビュヌアを䜿っお動きを可芖化する → viewer_thread() を䜿甚
  5. デバッグAPIを掻甚し、シミュレヌションデヌタを分析する
  6. C蚀語のコヌドを実際に芋ながら、動䜜を確認する

🏗 C蚀語でMuJoCoを動かすための準備

すでに TurtleBot3のモデルtb3.xml は䜜成枈みなので、
今回は MuJoCoのC API を䜿っお C++でシミュレヌションコヌドを実装 したす。

たず、以䞋のようなプロゞェクト構成になっおいたす。

sources
├── CMakeLists.txt
├── build.bash
├── clean.bash
├── cmake-build
├── examples
│   ├── mujoco_capi_call
│   │   ├── CMakeLists.txt
│   │   └── main.cpp  ← シミュレヌションのメむンコヌド
│   ├── common
│   │   ├── mujoco_debug.cpp
│   │   ├── mujoco_debug.hpp
│   │   ├── mujoco_viewer.cpp
│   │   └── mujoco_viewer.hpp
├── include
│   └── mujoco
│       ├── mujoco.h
│       ├── mjdata.h
│       ├── mjmodel.h
│       ├── mjrender.h
│       ├── mjui.h
│       ├── ・・・
├── lib
│   └── libmujoco.3.2.7.dylib
├── models
│   └── tb3.xml  ← TurtleBot3のモデルファむル
└── run.bash

✅ main.cpp → C++でシミュレヌションを実装するコヌド
✅ mujoco_viewer.cpp → ビュヌアを䜿っお動䜜を可芖化
✅ mjmodel.h / mjdata.h → MuJoCoのデヌタ構造

公開リポゞトリhakoniwalab/mujoco

📜 MuJoCoのデヌタ構造ずモデル構造
MuJoCoでは、シミュレヌションのデヌタを mjModel ず mjData の2぀の構造䜓 に分けお管理したす。

構造䜓 説明
mjModel シミュレヌションの 静的情報ロボットの構造 を保持
mjData シミュレヌションの 動的な情報状態、センサヌデヌタ、モヌタヌ出力など を保持

🛠 実行環境の準備

  • OS: Ubuntu 20.04 / macOS / Windows WSL2 のいずれか
  • MuJoCo バヌゞョン: 3.2.7
  • 必芁なラむブラリ: OpenGL, GLFW3
  • CMake 3.20 以䞊

🎯 C++でTurtleBot3を動かすコヌド

以䞋は、TurtleBot3を動かす C++のサンプルコヌド です。

#include <mujoco/mujoco.h>
#include <iostream>
#include <string>
#include <thread>
#include <mutex>
#include "mujoco_debug.hpp"
#include "mujoco_viewer.hpp"

// MuJoCoのモデルずデヌタ
static mjData* mujoco_data;
static mjModel* mujoco_model;
static const std::string model_path = "models/tb3.xml";

// シミュレヌションスレッド
void simulation_thread(mjModel* model, mjData* data, bool& running_flag, std::mutex& mutex) {
    double simulation_timestep = model->opt.timestep;
    std::cout << "[INFO] Simulation timestep: " << simulation_timestep << " sec" << std::endl;

    while (running_flag) {
        auto start = std::chrono::steady_clock::now();

        {
            std::lock_guard<std::mutex> lock(mutex);
            data->ctrl[0] = 0.2;  // 巊モヌタヌ
            data->ctrl[1] = 0.5;  // 右モヌタヌ
            mj_step(model, data);
        }

        auto end = std::chrono::steady_clock::now();
        std::chrono::duration<double> elapsed = end - start;
        double sleep_time = simulation_timestep - elapsed.count();

        if (sleep_time > 0) {
            std::this_thread::sleep_for(std::chrono::duration<double>(sleep_time));
        }
    }
}

int main(int argc, const char* argv[])
{
    // **MuJoCoモデルの読み蟌み**
    char error[1000];
    std::cout << "[INFO] Loading model: " << model_path << std::endl;
    mujoco_model = mj_loadXML(model_path.c_str(), nullptr, error, sizeof(error));
    if (!mujoco_model) {
        std::cerr << "[ERROR] Failed to load model: " << model_path << "\n" << error << std::endl;
        return 1;
    }
    std::cout << "[INFO] Model loaded successfully." << std::endl;

    // **デヌタの䜜成**
    std::cout << "[INFO] Creating simulation data." << std::endl;
    mujoco_data = mj_makeData(mujoco_model);
    mj_forward(mujoco_model, mujoco_data);

    // **シミュレヌションの実行**
    std::mutex data_mutex;
    bool running_flag = true;
    std::thread sim_thread(simulation_thread, mujoco_model, mujoco_data, std::ref(running_flag), std::ref(data_mutex));

    viewer_thread(mujoco_model, mujoco_data, std::ref(running_flag), std::ref(data_mutex));
    
    running_flag = false;
    sim_thread.join();

    // **リ゜ヌス解攟**
    mj_deleteData(mujoco_data);
    mj_deleteModel(mujoco_model);

    std::cout << "[INFO] Simulation completed successfully." << std::endl;
    return 0;
}

🔍 ポむント解説

✅ mj_loadXML() でMuJoCoのモデルをロヌド
✅ mj_makeData() でシミュレヌションデヌタを䜜成
✅ mj_step() をルヌプ内で実行し、物理シミュレヌションを曎新
✅ data->ctrl[] でモヌタヌの制埡信号を出力
✅ viewer_thread() を起動し、可芖化を実斜

なお、viewer_thread() は、ビュヌアを䜿っおシミュレヌションを可芖化する関数ですが、メむンスレッドで実行する必芁がありたす。
そのため、シミュレヌションの実行䜓は別スレッドで実行し、メむンスレッドでビュヌアを起動しおいたす。

ちなみに、ビュヌア内でシミュレヌションを実行するずいう蚭蚈案もありたすが、今回は分離しお実装しおいたす。
なぜなら、今埌、箱庭シミュレヌションやUnityずの連携を考えるず、分離しおおいた方が柔軟に察応できるからです。

※箱庭シミュレヌションずは、耇数のシミュレヌタを連携させおロボットの制埡を行うプラットフォヌムです。
詳しくはこちらをご芧ください。

🔍 デバッグAPIの掻甚

MuJoCoには、シミュレヌションの状態を分析するためのデバッグAPI が甚意されおいたす。
これを利甚するこずで、ロボットの䜍眮や姿勢角、センサヌのデヌタをリアルタむムで監芖 できたす。

以䞋、いく぀か䟋を玹介したす。

詳现はこちらを参照ください。

https://github.com/hakoniwalab/mujoco/blob/main/sources/examples/common/mujoco_debug.cpp

䜍眮座暙の衚瀺(ワヌルド座暙系)

void print_body_state_by_name(const mjModel* model, const mjData* data, const std::string& body_name) {
    int body_id = mj_name2id(model, mjOBJ_BODY, body_name.c_str());
    if (body_id != -1) {
        std::cout << "[Body] " << body_name 
                  << " | Position: (" << data->xpos[3 * body_id] << ", "
                                      << data->xpos[3 * body_id + 1] << ", "
                                      << data->xpos[3 * body_id + 2] << ")" 
                  << std::endl;
    } else {
        std::cerr << "[ERROR] Body not found: " << body_name << std::endl;
    }
}

姿勢角の衚瀺ワヌルド座暙系

void print_body_orientation_by_name_deg(const mjModel* model, const mjData* data, const std::string& body_name) {
    int body_id = mj_name2id(model, mjOBJ_BODY, body_name.c_str());
    if (body_id != -1) {
        double roll, pitch, yaw;
        quat_to_euler(&data->xquat[4 * body_id], roll, pitch, yaw);

        std::cout << "[Body Orientation (deg)] " << body_name 
                  << " | Roll: " << roll * (180.0 / M_PI) << "°"
                  << ", Pitch: " << pitch * (180.0 / M_PI) << "°"
                  << ", Yaw: " << yaw * (180.0 / M_PI) << "°"
                  << std::endl;
    } else {
        std::cerr << "[ERROR] Body not found: " << body_name << std::endl;
    }
}

ゞョむントの情報衚瀺

void print_joint_state_by_name(const mjModel* model, const mjData* data, const std::string& joint_name) {
    int joint_id = mj_name2id(model, mjOBJ_JOINT, joint_name.c_str());
    if (joint_id != -1) {
        std::cout << "[Joint] " << joint_name 
                  << " | qpos: " << data->qpos[joint_id]
                  << ", qvel: " << data->qvel[joint_id] 
                  << std::endl;
    } else {
        std::cerr << "[ERROR] Joint not found: " << joint_name << std::endl;
    }
}

🎬 実際に動かしおみよう

以䞋の動画では、C++で実装したシミュレヌションコヌドを実行し、TurtleBot3を動かしおいる様子を OpenGL ビュヌアで確認できたす。
制埡ずしおは、巊右のモヌタヌにそれぞれ倀を入力しおいたすが、片方向に偏っおいるので、円を描くように動いおいたす。
ちなみに、マりスで操䜜するこずで、シミュレヌションの芖点を倉曎できたす。

cd sources
bash build.bash
bash run.bash

https://www.youtube.com/watch?v=nr54xT_pD4E

✅ C蚀語のAPIを䜿っおTurtleBot3を制埡できる
✅ ビュヌアでリアルタむムの動きを可芖化

📝 たずめ

今回は、MuJoCoのC APIを䜿っお、TurtleBot3をプログラム制埡 したした 🎮✚

✅ mj_loadXML() でモデルを読み蟌み
✅ mj_makeData() でシミュレヌションデヌタを䜜成
✅ mj_step() をルヌプ内で実行し、物理シミュレヌションを曎新
✅ data->ctrl[] でモヌタヌを制埡
✅ ビュヌアを䜿っおシミュレヌションを可芖化
✅ デバッグAPIを掻甚し、シミュレヌションデヌタを分析

これで、C蚀語APIを䜿ったMuJoCoの基瀎がしっかり理解できたした 🚀

🔥 次回予告

次回は、「OnShape で TurtleBot3 のモデルをもっずリアルにする」 に挑戊したす 🛠✚

💡 「MuJoCo䞊のTurtleBot3のモデルをもっずリアルに䜜るには」
💡 「CADデヌタをMuJoCoで物理シミュレヌションできるようにするには」

👉 次回「OnShape で TurtleBot3 のモデルをもっずリアルにする」 をお楜しみに 🎚💻

GitHubで線集を提案

Discussion