double MPC::getCost(){ pom2_ = (tau_k_.transpose())*R_*(tau_k_); return ((double)(pom1_(0)+pom2_(0))); }
void material_coordinates::update_current_xy_configuration(vector const& u) { x.row(0) = X.row(0) + u.transpose()(Eigen::seq(0, u.size() - 1, 2)); x.row(1) = X.row(1) + u.transpose()(Eigen::seq(1, u.size() - 1, 2)); }
void MPC::substCost(){ pom1_ = ((z_k_.transpose())-(r_k_.transpose()))*Q_*(z_k_-r_k_); }