概要

ここでは、通常GrxUIの操作で行っているシミュレーションを、C++によるプログラムで行う方法を、サンプルを用いて説明します。　
サンプルプログラムは、OpenHRP/sample/project/samplePD.xml プロジェクトを用いたシミュレーションと同じ動作をします。

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

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

モデルの読み込み

まず、ModelLoaderを用いてモデルファイルを読み込みます。 Model[0],model[1]には、床とロボットのモデルファイルへのパスを指定します。 

BodyInfo_var floor = loadBodyInfo(Model[0].c_str(), argc, argv);
BodyInfo_var body = loadBodyInfo(Model[1].c_str(), argc, argv);

CORBA初期化

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

CORBA::ORB_var orb;
orb = CORBA::ORB_init(argc, argv);

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

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);

OnlineViewer

OnlineViewerの取得を行います。 

OnlineViewer_var olv = getOnlineViewer(argc, argv);

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

olv->load(floor->name(), Model[0].c_str());
olv->load(body->name(), Model[1].c_str());
olv->clearLog();

DynamicsSimulator

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

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から指定した型を取得した後、種々の例外処理を行うものです。この関数を利用してDynamicsSimulatorFactoryの取得を行います。 

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を作成しています。
次に、DynamicsSimulatorの初期化を行います。 

dynamicsSimulator->registerCharacter(floor->name(), floor);			    
dynamicsSimulator->registerCharacter(body->name(), body);
dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);

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

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

DblSequence3 g;
g.length(3);
g[0] = 0.0;
g[1] = 0.0;
g[2] = world_gravity;
dynamicsSimulator->setGVector(g);

次に、ロボットの初期位置と姿勢を設定します。 

Vector3  waist_p;
Matrix33 waist_R;
waist_p = 0, 0, 0.7135;
waist_R = tvmet::identity<Matrix33>();
DblSequence trans;
trans.length(12);
for(int i=0; i<3; i++) trans[i]   = waist_p(i);
for(int i=0; i<3; i++){
	for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
}
dynamicsSimulator->setCharacterLinkData( body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );

関節の初期角度も設定します。 

DblSequence angle;
angle.length(29);
angle[0] = 0.0;         angle[1] = -0.0360373;  angle[2] = 0.0;         angle[3] = 0.0785047;
angle[4] = -0.0424675;  angle[5] = 0.0;         angle[6] = 0.174533;    angle[7] = -0.00349066;
angle[8] = 0.0;         angle[9] = -1.5708;     angle[10] = 0.0;        angle[11] = 0.0;
angle[12] = 0.0;        angle[13] = 0.0;        angle[14] = -0.0360373; angle[15] = 0.0;
angle[16] = 0.0785047;  angle[17] = -0.0424675; angle[18] = 0.0;        angle[19] = 0.174533;
angle[20] = -0.00349066;angle[21] = 0.0;        angle[22] = -1.5708;    angle[23] = 0.0;
angle[24] = 0.0;        angle[25] = 0.0;        angle[26] = 0.0;        angle[27] = 0.0;
angle[28] = 0.0;
dynamicsSimulator->setCharacterAllLinkData( body->name(), DynamicsSimulator::JOINT_VALUE, angle );

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

dynamicsSimulator->calcWorldForwardKinematics();

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

DblSequence6 K, C;    
K.length(0);
C.length(0);
dynamicsSimulator->registerCollisionCheckPair(floor->name(),"", body->name() ,"",
		                                            statFric,slipFric,K,C,culling_thresh);

最後に初期化メソッドを呼びます。 

dynamicsSimulator->initSimulation();

Controller

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

Controller_var controller;
controller = checkCorbaServer <Controller, Controller_var> ("SamplePDController", cxt);

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

この場合はSamplePDControllerという名前のControllerを取得しています。
Controllerの設定を行います。 

controller->setModelName(body->name());
controller->setDynamicsSimulator(dynamicsSimulator);
controller->initialize();
controller->setTimeStep(controlTimeStep);

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

controller->start();

シミュレーションループ

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

  1. Controller::inputの呼び出し（DynamicsSimulatorからControllerへロボットデータを入力）
  2. Controller::controlの呼び出し（Controllerの出力値の計算）
  3. dynamicsSimulator::stepSimulationの呼び出し　(シミュレーション1ステップ実行）
  4. dynamicsSimulator::getWorldStateの呼び出し　(シミュレーション結果の読み出し)
  5. OnlineViewer::updateの呼び出し　（OnlineViewerの更新)
  6. Controller::outputの呼び出し (ControllerからCynamicdSimulatorへ制御値を出力）

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

bool control=false;
if(controlTime <= time){
    control=true;
    j++;
}

if(control)
    controller->input();

i++;
time = timeStep * i;
controlTime = controlTimeStep * j;

if(control)
    controller->control();
               
// ================== simulate one step ==============
dynamicsSimulator->stepSimulation();					    
              
// ================== viewer update ====================
try {
    dynamicsSimulator -> getWorldState( state );
    olv->update( state );
} catch (CORBA::SystemException& ex) {
    return 1;
}

// ===================== get robot status ===================
DblSequence_var waist_pR;  // position and attitude
DblSequence_var waist_vw;  // linear and angular velocities
dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, waist_pR);
dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_VELOCITY,  waist_vw);

// ================== log data save =====================
log_file << time << " ";
log_file << waist_vw[2] << " ";
log_file << endl;

if(control)
    controller->output();

if( time > EndTime ) break;

実行方法

  1. GrxUIを起動。
  2. OpenHRP/sample/controller/SamplePD/SamplePD.sh (.bat) をコマンドラインから実行。
  3. OpenHRP/sample/example/scheduler/scheduler.sh (.bat) をコマンドラインから実行。

Windowsでコマンドラインから実行する時は、(OpenHRPのインストールディレクトリ)/bin にパスを通してください。