新たなロボットをmc_rtcでサポートする方法

import mc_rbdyn

path = "/path/to/description"
name = "name"
env = mc_rbdyn.get_robot_module("env", path, name)

# オブジェクトによってロボットと浮遊ベースが読み込まれる
object = mc_rbdyn.get_robot_module("object", path, name)

["env", "/path/to/description", "name"]

// オブジェクトによってロボットと浮遊ベースが読み込まれる
["object", "/path/to/description", "name"]

[env, /path/to/description, name]

# オブジェクトによってロボットと浮遊ベースが読み込まれる
[object, /path/to/description, name]

import mc_rbdyn

json_path = "/path/to/file.json"
rm_json = mc_rbdyn.get_robot_module("json", json_path)

yaml_path = "/path/to/file.yaml"
rm_yaml = mc_rbdyn.get_robot_module("json", yaml_path)

["json", "/path/to/file.json"]

["json", "/path/to/file.yaml"]

[json, /path/to/file.json]

[json, /path/to/file.yaml]

#pragma once

#include <mc_rbdyn/RobotModule.h>
#include <mc_robots/api.h>

struct MC_ROBOTS_DLLAPI MyRobotModule : public mc_rbdyn::RobotModule
{
public:
  MyRobotModule(bool fixed);
};

extern "C"
{
  ROBOT_MODULE_API void MC_RTC_ROBOT_MODULE(std::vector<std::string> & names)
  {
    // このモジュールによってエクスポートされるロボットの名前
    names = {"MyRobot", "MyRobotFixed"};
  }
  ROBOT_MODULE_API void destroy(mc_rbdyn::RobotModule * ptr)
  {
    delete ptr;
  }
  ROBOT_MODULE_API mc_rbdyn::RobotModule * create(const std::string & name)
  {
    // 現時点ではサポートされているいずれかのロボットの名前を使用する必要がある
    if(name == "MyRobot") { return new MyRobot(false); }
    else
    {
      return new MyRobot(true);
    }
  }
}

#include "MyRobot.h"

#include <RBDyn/parsers/urdf.h>

// MY_PATHはCMakeなどで与えることができる
MyRobotModule::MyRobotModule(bool fixed) : mc_rbdyn::RobotModule(MY_PATH, "my_robot")
{
  init(rbd::parsers::from_urdf_file(urdf_path));

  std::vector<double> default_q = {-0.38, -0.01, 0., 0.72, -0.01, -0.33,  -0.38, 0.02, 0.,    0.72, -0.02, -0.33, 0.,
                                   0.13,  0.,    0., 0.,   0.,    -0.052, -0.17, 0.,   -0.52, 0.,   0.,    0.,    0.,
                                   0.,    0.,    0., 0.,   0.,    -0.052, 0.17,  0.,   -0.52, 0.,   0.,    0.,    0.,
                                   0.,    0.,    0., 0.,   0.,    0.,     0.,    0.,   0.,    0.,   0.};
  const auto & rjo = ref_joint_order();
  for(size_t i = 0; i < rjo.size(); ++i) { _stance[rjo[i]] = {default_q[i]}; }
  _default_attitude = {{1., 0., 0., 0., 0., 0., 0.8275}};
  _forceSensors.push_back(mc_rbdyn::ForceSensor("RightFootForceSensor", "R_ANKLE_P_S", sva::PTransformd::Identity()));
  _forceSensors.push_back(mc_rbdyn::ForceSensor("LeftFootForceSensor", "L_ANKLE_P_S", sva::PTransformd::Identity()));
  _forceSensors.push_back(mc_rbdyn::ForceSensor("RightHandForceSensor", "R_WRIST_Y_S", sva::PTransformd::Identity()));
  _forceSensors.push_back(mc_rbdyn::ForceSensor("LeftHandForceSensor", "L_WRIST_Y_S", sva::PTransformd::Identity()));

  _bodySensors.emplace_back("Accelerometer", "PELVIS_S", sva::PTransformd(Eigen::Vector3d(-0.0325, 0, 0.1095)));
  _bodySensors.emplace_back("FloatingBase", "PELVIS_S", sva::PTransformd::Identity());

  _minimalSelfCollisions = {
      {"WAIST_R_S", "L_SHOULDER_Y_S", 0.02, 0.001, 0.}, {"WAIST_R_S", "R_SHOULDER_Y_S", 0.02, 0.001, 0.},
      {"PELVIS_S", "R_ELBOW_P_S", 0.05, 0.001, 0.},     {"PELVIS_S", "L_ELBOW_P_S", 0.05, 0.001, 0.},
      {"R_WRIST_Y_S", "R_HIP_Y_S", 0.05, 0.025, 0.},    {"L_WRIST_Y_S", "L_HIP_Y_S", 0.05, 0.025, 0.}};
  _commonSelfCollisions = _minimalSelfCollisions;
  _grippers = {{"l_gripper", {"L_UTHUMB"}, true}, {"r_gripper", {"R_UTHUMB"}, false}};
}