base::Vector6d NMPC::calcControl(const base::Vector6d &reference,
const base::VectorXd &pose_samples,
const base::Vector6d &efforts)
{
/* Get the time before start of the loop */
acado_timer t;
acado_tic( &t );
calcTrajectory(reference);
updateStates(pose_samples, efforts);
/* Prepare first step */
acado_preparationStep();
/* The "real-time iterations" loop */
for(int iter = 0; iter < num_steps_; ++iter)
{
acado_feedbackStep( );
// Optional: shift the initialization (look at acado_common.h)
// shiftStates(2, 0, 0);
// shiftControls( 0 );
/* Prepare for the next step */
acado_preparationStep();
}
base::Vector6d control = base::Vector6d::Zero();
for (int i = 0; i < kNumControls; i++)
control[i] = acadoVariables.u[i];
control_derivative_ = control;
/* Read the elapsed time */
elapsed_time_ = acado_toc( &t );
return control;
}
/**
* \brief
* Update movement information.
*
* \note
* Should not be overridden or shadowed by deriving classes.
*/
void makeTrajectory()
{
addCapturable(pos);
calcTrajectory();
}
