/*************************************************************
* ANOTHER FUNCTIONS
*************************************************************/
void HingeEncoders::init ( const std::string newName,
AL::ALPtr<AL::ALBroker> parentBroker ) {
this->name = newName;
try {
this->motion = parentBroker->getMotionProxy();
} catch ( AL::ALError& e ) {
std::cerr << "[HingeEncoders ()::init(): " << e.toString() << std::endl;
}
}
void
Kick::init(const string newName, AL::ALPtr<AL::ALBroker> parentBroker)
{
Component::init(newName, parentBroker);
fin = false;
try{
pmotion = parentBroker->getMotionProxy();
}catch( AL::ALError& e) {
cerr << "[Kick ()::init(): " << e.toString() << endl;
}
}
void
RecorderSession::init(const string newName, AL::ALPtr<AL::ALBroker> parentBroker)
{
Component::init(newName, parentBroker);
setFreqTime(SHORT_RATE);
try
{
pmotion = parentBroker->getMotionProxy();
lastpos = pmotion->getRobotPosition(USE_SENSOR);
} catch (AL::ALError& e) {
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
cerr << "Recorder::NEKFLocalization [motion]" << e.toString() << endl;
}
initts = lastts = getCurrentTime();
}
void EKFLocalization::init(const string newName,
AL::ALPtr<AL::ALBroker> parentBroker) {
Component::init(newName, parentBroker);
setFreqTime(MEDIUM_RATE);
ekf.filter = new EKF_3D();
MatrixCM sinit, Pinit;
sinit.resize(3, 1);
sinit.sete(0,0,0.0);
sinit.sete(1,0,0.0);
sinit.sete(2,0,0.0);
Pinit.identity(3);
Pinit.sete(0, 0, 3000.0*3000.0);
Pinit.sete(1, 1, 2000.0*2000.0);
Pinit.sete(2, 2, M_PI * M_PI);
ekf.filter->restart(sinit, Pinit);
mgrid = new MarkovGrid();
try
{
pmotion = parentBroker->getMotionProxy();
lastpos = pmotion->getRobotPosition(USE_SENSOR);
} catch (AL::ALError& e) {
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
lastpos.push_back(0.0);
cerr << "EKFLocalization::EKFLocalization [motion]" << e.toString() << endl;
}
// lastpos = pmotion->getRobotPosition(USE_SENSOR);
//_MovementModel->initMovement();
}