サンプル
ソースコード
#include <stdio.h>
#include <string>
#include <OnlineViewerClient.h>
#include <DynamicsSimulator.h>
using namespace std;
using namespace OpenHRP;
enum {X, Y, Z};
#define deg2rad(x) ( 3.14159265358979 / 180*(x) )
#define DOF 29 // 自由度
// サーバ群の取得
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;
break;
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;
}
int main(int argc, char* argv[])
{
int i;
string url = "file://";
// -urlでモデルのURLを指定
for(i=0; i < argc; i++)
{
if( strcmp(argv[i], "-url") == 0 && i+1 < argc) url += argv[i+1];
}
string robot_url = url+"sample.wrl";
string floor_url = url+"floor.wrl";
const char *ROBOT_URL = robot_url.c_str();
const char *FLOOR_URL = floor_url.c_str();
//////////////////////////////////////////////////////////////////////
// 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();
// NamingServiceの参照取得
CosNaming::NamingContext_var cxT;
{
CORBA::Object_var nS = orb->resolve_initial_references("NameService");
cxT = CosNaming::NamingContext::_narrow(nS);
}
/////////////////////////////////////////////////////////////////////////
// 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();
// ModelLoader取得
ModelLoader_var modelLoader =
checkCorbaServer <ModelLoader,
ModelLoader_var> ("ModelLoader", cxT);
if (CORBA::is_nil(modelLoader))
{
std::cerr << "ModelLoader not found" << std::endl;
}
// モデルの読み込み・登録
CharacterInfo_ptr cInfo;
try
{
cInfo = modelLoader->loadURL(ROBOT_URL);
}
catch(CORBA::SystemException& ex)
{
std::cerr << "CORBA::SystemException raised by ModelLoader: "
<< ex._rep_id() << std::endl;
}
catch(ModelLoader::ModelLoaderException& ex)
{
std::cerr << "ModelLoaderException (id = " << ex.id << ") : "
<< ex.description << std::endl;
}
dynamicsSimulator->registerCharacter("robot", cInfo);
// 床の読み込み・登録
try
{
cInfo = modelLoader->loadURL(FLOOR_URL);
}
catch(CORBA::SystemException& ex)
{
std::cerr << "CORBA::SystemException raised by ModelLoader: "
<< ex._rep_id() << std::endl;
}
catch(ModelLoader::ModelLoaderException& ex)
{
std::cerr << "ModelLoaderException (id = " << ex.id << ") : "
<< ex.description << std::endl;
}
dynamicsSimulator->registerCharacter("floor", cInfo);
/////////////////////////////////////////////////////////////////////////
// DynamicsSimulatorの初期化
dynamicsSimulator->init(0.002,
DynamicsSimulator::RUNGE_KUTTA,
DynamicsSimulator::ENABLE_SENSOR);
// 重力ベクトルの設定
DblSequence3 gVector;
gVector.length(3);
gVector[0] = gVector[1] = 0;
gVector[2] = 9.8;
dynamicsSimulator->setGVector(gVector);
// 関節駆動モードの設定
dynamicsSimulator->setCharacterAllJointModes(
"robot", DynamicsSimulator::TORQUE_MODE);
// 初期姿勢
double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00,
1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00,
0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02,
0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00,
0.00E+00, 0.00E+00, 0, 0, 0};
// 初期姿勢を関節角にセット
DblSequence q;
q.length(DOF);
for (int i=0; i<DOF; i++)
{
q[i] = init_pos[i];
}
dynamicsSimulator->setCharacterAllLinkData(
"robot", DynamicsSimulator::JOINT_VALUE, q);
// 順運動学計算
dynamicsSimulator->calcWorldForwardKinematics();
// 干渉チェックペアの設定－両手
DblSequence6 dc, sc;
dc.length(0);
sc.length(0);
dynamicsSimulator->registerCollisionCheckPair(
"robot",
"RARM_WRIST_R",
"robot",
"LARM_WRIST_R",
0.5,
0.5,
dc,
sc);
dynamicsSimulator->initSimulation();
/////////////////////////////////////////////////////////////////////////
// OnlineViewerの取得
OnlineViewer_var onlineViewer =
checkCorbaServer <OnlineViewer,
OnlineViewer_var> ("OnlineViewer", cxT);
if (CORBA::is_nil(onlineViewer))
{
std::cerr << "OnlineViewer not found" << std::endl;
}
// OnlineViewerに対するモデルロード
try
{
onlineViewer->load("robot", ROBOT_URL);
onlineViewer->load("floor", FLOOR_URL);
onlineViewer->clearLog();
}
catch (CORBA::SystemException& ex)
{
std::cerr << "Failed to connect GrxUI." << endl;
return 1;
}
/////////////////////////////////////////////////////////////////////////
// 逆運動学計算の準備
double RARM_p[] = {0.197403, -0.210919, 0.93732};
double RARM_R[] = {0.174891, -0.000607636, -0.984588,
0.00348999, 0.999994, 2.77917e-06,
0.984582, -0.00343669, 0.174892};
double LARM_p[] = {0.197403, 0.210919, 0.93732};
double LARM_R[] = {0.174891, 0.000607636, -0.984588,
-0.00348999, 0.999994, -2.77917e-06,
0.984582, 0.00343669, 0.174892};
double dp = 0.0;
/////////////////////////////////////////////////////////////////////////
WorldState_var state;
while (1)
{
// 逆運動学計算
LinkPosition link;
link.p[0] = RARM_p[0];
link.p[1] = RARM_p[1] + dp;
link.p[2] = RARM_p[2];
for (int i=0; i<9; i++)
link.R[i] = RARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
CORBA::string_dup("CHEST"),
CORBA::string_dup("RARM_WRIST_R"),
link);
link.p[0] = LARM_p[0];
link.p[1] = LARM_p[1] - dp;
link.p[2] = LARM_p[2];
for (int i=0; i<9; i++)
link.R[i] = LARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
CORBA::string_dup("CHEST"),
CORBA::string_dup("LARM_WRIST_R"),
link);
dynamicsSimulator->calcWorldForwardKinematics();
dp += 0.001;
// OnlineViewer更新
try
{
dynamicsSimulator->getWorldState(state);
state->time = dp*10;
onlineViewer->update(state);
}
catch (CORBA::SystemException& ex)
{
std::cerr << "OnlineViewer could not be updated." << endl;
return 1;
}
// 干渉チェック
dynamicsSimulator->checkCollision();
if (state->collisions.length() > 0)
{
if (state->collisions[0].points.length() > 0)
{
break;
}
}
}
return 0;
}
