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(); }