ð» MuJoCo Cèšèªã®API㧠TurtleBot3 ãåãããŠã¿ãïŒ
ä»åã¯ãCèšèªã®APIã䜿ã£ãŠããã°ã©ã ã§å¶åŸ¡ããTurtleBot3ãèªåã§åãããŠã¿ãŸãïŒ ð»âïž
ð ååã®ãããã
ååã®èšäº ã§ã¯ãTurtleBot3ã®ã¢ãã«ã«ä»¥äžã®èŠçŽ ãè¿œå ããŸããã
â
freejoint ãè¿œå ããéåã®åœ±é¿ãåããããã«ãã
â
hinge joint ãè¿œå ããè»èŒªã®å転ãå¯èœã«ãã
â
ãã£ã¹ã¿ãŒã3軞ã§èªç±å転ã§ããããã«ãã
â
motor ãè¿œå ããè»èŒªã®é§åãå¶åŸ¡ã§ããããã«ãã
ãããŠãMuJoCoã®ã·ãã¥ã¬ãŒã·ã§ã³äžã§ ãã¢ãŒã¿ãŒã«æ°å€ãå ¥åãããšåãã ããšã確èªããŸããã
ãããããŸã æåã§å€ãå
¥åããå¿
èŠããããŸããã
次㯠Cèšèªã§APIãçŽæ¥æäœããããã°ã©ã ããTurtleBot3ãåãã ããšã«ææŠããŸãïŒ ð
ð§ CèšèªAPIã䜿ã£ãã·ãã¥ã¬ãŒã·ã§ã³ã®æµã
MuJoCoã® CèšèªAPI ã䜿ãã以äžã®æµãã§TurtleBot3ãå¶åŸ¡ããŸãã
- MuJoCoã¢ãã«ïŒtb3.xmlïŒãèªã¿èŸŒã â mj_loadXML() ã䜿çš
- ã·ãã¥ã¬ãŒã·ã§ã³çšã®ããŒã¿æ§é ãäœæ â mj_makeData() ã䜿çš
- APIã䜿ã£ãŠã¢ãŒã¿ãŒã«æ瀺ãåºã â data->ctrl[] ãèšå®
- ãã¥ãŒã¢ã䜿ã£ãŠåããå¯èŠåãã â viewer_thread() ã䜿çš
- ãããã°APIã掻çšããã·ãã¥ã¬ãŒã·ã§ã³ããŒã¿ãåæãã
- 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 ãçšæãããŠããŸãã
ãããå©çšããããšã§ãããããã®äœçœ®ã姿å¢è§ãã»ã³ãµãŒã®ããŒã¿ããªã¢ã«ã¿ã€ã ã§ç£èŠ ã§ããŸãã
以äžãããã€ãäŸã玹ä»ããŸãã
詳现ã¯ãã¡ããåç §ãã ããã
äœçœ®åº§æšã®è¡šç€º(ã¯ãŒã«ã座æšç³»)
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
â
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 ã®ã¢ãã«ããã£ãšãªã¢ã«ã«ããïŒã ãã楜ãã¿ã«ïŒ ðšð»
Discussion