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;
}
void MeshShapeItemImpl::onUpdated()
{
sceneLink->translation() = self->absTranslation;
sceneLink->rotation() = self->absRotation;
if (path != ""){
if (shape) {
sceneLink->removeChild(shape);
shape = NULL;
}
BodyLoader bodyLoader;
BodyPtr newBody = bodyLoader.load(path);
if (!newBody) return;
shape = newBody->rootLink()->visualShape();
sceneLink->addChildOnce(shape);
sceneLink->notifyUpdate();
}
}
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;
}
}
}
