RTC::ReturnCode_t PA10PickupControllerRTC::onInitialize()
{
// Set InPort buffers
addInPort("q", m_angleIn);
addInPort("u_in", m_torqueIn);
// Set OutPort buffer
addOutPort("u_out", m_torqueOut);
string modelfile = getNativePathString(
boost::filesystem::path(shareDirectory()) / "model/PA10/PA10.wrl");
BodyLoader loader;
loader.setMessageSink(cout);
loader.setShapeLoadingEnabled(false);
body = loader.load(modelfile);
if(!body){
cout << modelfile << " cannot be loaded." << endl;
return RTC::RTC_ERROR;
}
n = body->numJoints();
leftHand_id = body->link("HAND_L")->jointId();
rightHand_id = body->link("HAND_R")->jointId();
return RTC::RTC_OK;
}
SimpleControllerWrapper(const char* charname)
: OpenHRPControllerBase(charname)
{
ready = false;
controller = createSimpleController();
if(controller){
string modelfile = getNativePathString(
boost::filesystem::path(shareDirectory()) / "model" / MODELFILE);
BodyLoader loader;
loader.setMessageSink(cout);
loader.setShapeLoadingEnabled(false);
ioBody = loader.load(modelfile);
if(ioBody){
ready = true;
} else {
cout << modelfile << " cannot be loaded." << endl;
}
}
}