スケジューラ作成法(Ver.3.0)

概要

ここでは、通常GrxUIが行うModelLoaderへのモデル読み込み、DynamicsServerでのシミュレーション、Controllerでの制御をスケジューラを作成することで説明します。

流れは以下のとおりです。

1. CORBA初期化
2. NamingServiceを取得する
3. ModelLoaderを取得し、モデルを読み込む
4. DynamicsServerを取得し、シミュレーション設定を行う
5. OnlineViewerを取得し、モデルを読み込む
6. Controllerを取得する
7. シミュレーションのループ

CORBA初期化

最初にCORBA ORBの初期化とNamingServiceの参照取得を行います。

// CORBA初期化
CORBA::ORB_var orb;
orb = CORBA::ORB_init(argc, argv);

// ROOT POA
CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);

// POAマネージャへの参照を取得
PortableServer::POAManager_var manager = rootPOA -> the_POAManager();

CosNaming::NamingContext_var cxT;
CORBA::Object_var	nS = orb->resolve_initial_references("NameService");
cxT = CosNaming::NamingContext::_narrow(nS);

ModelLoader取得

まず、サーバ群を取得するために以下のような関数を用意します。

template <typename X, typename X_ptr>
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
{
  CosNaming::Name ncName;
  ncName.length(1);
  ncName[0].id = CORBA::string_dup(n.c_str());
  ncName[0].kind = CORBA::string_dup("");
  X_ptr srv = NULL;
  try {
    srv = X::_narrow(cxt->resolve(ncName));
  } catch(const CosNaming::NamingContext::NotFound &exc) {
    std::cerr << n << " not found: ";
    switch(exc.why) {
    case CosNaming::NamingContext::missing_node:
      std::cerr << "Missing Node" << std::endl;
    case CosNaming::NamingContext::not_context:
      std::cerr << "Not Context" << std::endl;
      break;
    case CosNaming::NamingContext::not_object:
      std::cerr << "Not Object" << std::endl;
      break;
    }
    return (X_ptr)NULL;
  } catch(CosNaming::NamingContext::CannotProceed &exc) {
    std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
    std::cerr << "Resolve " << n << " InvalidName" << std::endl;
  }
  return srv;
}

これはNamingServiceから指定した型を取得した後、種々の例外処理を行うものです。この関数を利用してModelLoaderとDynamicsSimulatorの取得を行います。

DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
dynamicsSimulatorFactory =
  checkCorbaServer <DynamicsSimulatorFactory,
  DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxT);

if (CORBA::is_nil(dynamicsSimulatorFactory)) {
  std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
}

DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();

ここでは、まずDynamicsSimulatorFactoryを取得し、それに対しcreateメソッドを使用することでDynamicsSimulatorを起こしています。

次にModelLoaderの取得です。

ModelLoader_var modelLoader =
  checkCorbaServer <ModelLoader,
  ModelLoader_var> ("ModelLoader", cxT);

if (CORBA::is_nil(modelLoader)) {
  std::cerr << "ModelLoader not found" << std::endl;
}

取得したModelLoaderとDynamicsSimulatorに対してモデルの読み込みを行います。

CharacterInfo_ptr cInfo = modelLoader->loadURL(ROBOT_URL);
dynamicsSimulator->registerCharacter("robot", cInfo);

1行目でモデルをModelLoaderに対して読み込ませ、2行目でキャラクタ"robot"として登録しています。同様に床も登録します。

cInfo = modelLoader->loadURL(FLOOR_URL);
dynamicsSimulator->registerCharacter("floor", cInfo);

DynamicsSimulator

DynamicsSimulatorの初期化を行います。

dynamicsSimulator->init(0.002, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);

initメソッドはシミュレーション1ステップごとの時間、差分法、センサーの有効/無効を設定します。 差分法にはオイラー法を用いるDynamicsSimulator::EULERとルンゲクッタ法を用いるDynamicsSimulator::RUNGE_KUTTAが設定できます。 センサーは有効にするDynamicsSimulator::ENABLE_SENSORと無効にするDynamicsSimulator::DISABLE_SENSORがあります。

次に、重力ベクトルを設定します。

DblSequence3 gVector;
gVector.length(3);
gVector[0] = gVector[1] = 0;
gVector[2] = 9.8;
dynamicsSimulator->setGVector(gVector);

この場合、下向きに9.8ms^-2の重力加速度がかかることになります。

さらに、関節駆動モードの設定を行います。

dynamicsSimulator->setCharacterAllJointModes("robot", DynamicsSimulator::TORQUE_MODE);

このコードでキャラクタ"robot"の全関節に対してトルクモードが設定されます。 関節モードは上記で使用したDynamicsSimulator::TORQUE_MODEのほかにハイゲインモードDynamicsSimulator::HIGH_GAIN_MODEもあります。

次に、ロボットの初期姿勢を決定します。 DynamicsSimulator::setCharacterAllLinkData等が使用できますので、それを用いて設定してください。

設定が終わったら順運動学計算を１回行い、姿勢に反映させます。

dynamicsSimulator->calcWorldForwardKinematics();

次に衝突検出ペアの登録を行います。ここで登録されたペア同士の衝突検出が行われることになります。ロボットと床の間にペアを設定しています。

DblSequence6 sc, dc;
sc.length(0);
dc.length(0);
dynamicsSimulator->registerCollisionCheckPair
  ("robot",
   "",
   "floor",
   "",
   0.5,
   0.5,
   sc,
   dc);

最後にinitSimulationメソッドを呼びます。

dynamicsSimulator->initSimulation();

OnlineViewer

OnlineViewerの取得を行います。

OnlineViewer_var onlineViewer =
  checkCorbaServer <OnlineViewer,
  OnlineViewer_var> ("OnlineViewer", cxT);

if (CORBA::is_nil(onlineViewer)) {
  std::cerr << "OnlineViewer not found" << std::endl;
}

さらに、OnlineViewerに対してもモデルを読み込ませます。

onlineViewer->load("robot", ROBOT_URL);
onlineViewer->load("floor", FLOOR_URL);

Controller

ControllerFactoryを取得する際はそれぞれのController名で行います。

ControllerFactory_var controllerFactory;
controllerFactory =
  checkCorbaServer <ControllerFactory,
  ControllerFactory_var> ("PDcontroller", cxT);

if (CORBA::is_nil(controllerFactory)) {
  std::cerr << "ControllerFactory not found" << std::endl;
}

この場合はPDcontrollerという名前のControllerのControllerFactoryを取得しています。

FactoryからControllerを取得します。その際にどのキャラクタのControllerかも指示します。

Controller_var controller = controllerFactory->create("robot");

また、DynamicsSimulatorもControllerにセットします。

controller->setDynamicsSimulator(dynamicsSimulator);

最後にスタートさせます。

controller->start();

シミュレーションループ

実際のシミュレーション実行時にまわされるループです。以下の構成を持ちます。

1. OnlineViewerのアップデート
2. Controller::inputの呼び出し
3. Controller::controlの呼び出し
4. DynamicsSimulator::stepSimulationの呼び出し
5. Controller::outputの呼び出し

具体的な形は以下のようになります。

WorldState_var state;
while (1) {
  // WorldStateを取得し、OnlineViewerに渡す
  dynamicsSimulator->getWorldState(state);
  onlineViewer->update(state);

  // input呼び出し
  controller->input();

  // control呼び出し
  controller->control();

  // stepSimulationを呼び出し、シミュレーションを進めます。
  dynamicsSimulator->stepSimulation();

  // output呼び出し
  controller->output();
}

付加情報

上記シミュレーションループにおいては衝突による法線ベクトルが表示されますが、法線ベクトルを編集し他の目的に使用することもできます。 これは、WorldStateのcollisionsを以下のように編集することで実現できます。

dynamicsSimulator->getWorldState(state);

state->collisions.length(1);
state->collisions[0].pair.charName1 = CORBA::string_dup("robot");
state->collisions[0].pair.charName2 = CORBA::string_dup("floor");
state->collisions[0].pair.linkName1 = CORBA::string_dup("RLEG_ANKLE_R");
state->collisions[0].pair.linkName2 = CORBA::string_dup("WAIST");
state->collisions[0].points.length(1);
state->collisions[0].points[0].position[0] = 0.0;
state->collisions[0].points[0].position[1] = 0.0;
state->collisions[0].points[0].position[2] = 0.0;
state->collisions[0].points[0].normal[0] = 0.0;
state->collisions[0].points[0].normal[0] = 0.0;
state->collisions[0].points[0].normal[0] = 0.5;

onlineViewer->update(state);

このコードはキャラクタ名robot, floorの間に(0.0, 0.0, 0.0)から(0.0, 0.0, 0.5)の法線ベクトルを描画するように設定されます。