ompl::control::Control*
RHCICreate::generateFeedbackControl(const ompl::base::State *state, const size_t& _t)
{
using namespace arma;
//if no more controls left, regenerate controls
if(openLoopControls_.size() == 0) {
std::vector<ompl::control::Control*> openLoopControls;
this->motionModel_->generateOpenLoopControls(state , this->goal_, openLoopControls) ;
openLoopControls_ = std::deque<ompl::control::Control*>(openLoopControls.begin(), openLoopControls.end());
//if motion model cannot generate valid open loop controls from start to goal, return an empty vector signifying invalid control
if(openLoopControls_.size() == 0) {
return this->motionModel_->getZeroControl();
}
//if we generate more controls than the length of controlQueueSize (user-defined), we truncate the rest
if(openLoopControls_.size() > controlQueueSize_)
openLoopControls_.resize(controlQueueSize_);
}
// if there are controls left, apply and remove them one-by-one
ompl::control::Control* control;
control = openLoopControls_.front();
openLoopControls_.pop_front();
colvec diff = (this->goal_)->as<StateType>()->getArmaData() - state->as<StateType>()->getArmaData();
double distance = norm(diff.subvec(0,1), 2);
SpaceType *space;
space = new SpaceType();
ompl::base::State *relativeState = space->allocState();
space->getRelativeState(state, goal_, relativeState);
colvec relativeCfg = relativeState->as<StateType>()->getArmaData();
//cout<<"RHCICreate controller, goal_: "<<endl<<this->goal_.GetArmaData()<<endl;
//cout<<"RHCICreate controller, relativeCfg bearing (degrees): "<<relativeCfg[2]*180/PI<<endl;
//cout<<"RHCICreate controller, relativeCfg distance (cm ) : "<<distance*100<<endl;
//if( distance < 0.20 && !this->m_reachedFlag) this->m_reachedFlag = true;
if(distance < turnOnlyDistance_){
ompl::control::Control* newcontrol = this->motionModel_->getZeroControl();
//cout<<"Applying Only Turn Control !"<<endl;
if (abs(relativeCfg[2]) > 1e-6)
{
//cout<<"control val: "<<0.2 * relativeCfg[2]/abs(relativeCfg[2])<<endl;
//SO std::cin.get();
newcontrol->as<ompl::control::RealVectorControlSpace::ControlType>()->values[1] = 0.2 * relativeCfg[2]/abs(relativeCfg[2]);
}
return newcontrol;
}
return control;
}
