Collects the largest equality and inequality constraint violations from acados.
118 {
119
120
121 for (int stage = 0; stage <= dims->N; ++stage) {
122 std::vector<double> residuals(2 * dims->ni[stage]);
123 ocp_nlp_get_at_stage(solver, stage, "ineq_fun", residuals.data());
124 for (size_t index = 0; index < residuals.size(); ++index) {
125 if (residuals[index] > metrics.max_ineq_violation) {
126 metrics.max_ineq_violation = residuals[index];
127 metrics.max_ineq_stage = stage;
128 metrics.max_ineq_index = static_cast<int>(index);
129 }
130 }
131 }
132
133 for (int stage = 0; stage < dims->N; ++stage) {
134 std::vector<double> residuals(dims->nx[stage + 1]);
135 ocp_nlp_get_at_stage(solver, stage, "res_eq", residuals.data());
136 for (size_t state = 0; state < residuals.size(); ++state) {
137 if (std::abs(residuals[state]) > metrics.max_eq_violation) {
138 metrics.max_eq_violation = std::abs(residuals[state]);
139 metrics.max_eq_stage = stage;
140 metrics.max_eq_state = static_cast<int>(state);
141 }
142 }
143 }
144
145 if (metrics.max_ineq_stage < 0) return;
146
147 const int ni = dims->ni[metrics.max_ineq_stage];
148 const int nb = dims->nb[metrics.max_ineq_stage];
149 const int ng = dims->ng[metrics.max_ineq_stage];
150 const int nh = ni - nb - ng - dims->ns[metrics.max_ineq_stage];
151 const int index = metrics.max_ineq_index % ni;
152 metrics.max_ineq_side = metrics.max_ineq_index < ni ? "lower" : "upper";
153 metrics.max_ineq_index = index;
154 if (index < nb) {
155 std::vector<int> bound_indices(nb);
156 ocp_nlp_get_at_stage(solver, metrics.max_ineq_stage, "idxb", bound_indices.data());
157 const int variable_index = bound_indices[index];
158 if (variable_index < dims->nu[metrics.max_ineq_stage]) {
159 metrics.max_ineq_type = "control";
160 metrics.max_ineq_index = variable_index;
161 } else {
162 metrics.max_ineq_type = "state";
163 metrics.max_ineq_index = variable_index - dims->nu[metrics.max_ineq_stage];
164 }
165 } else if (index < nb + ng) {
166 metrics.max_ineq_type = "linear";
167 } else if (index < nb + ng + nh) {
168 const int h_index = index - nb - ng;
169 if (h_index < 2) {
170 metrics.max_ineq_type = h_index == 0 ? "boundary_left" : "boundary_right";
171 metrics.max_ineq_index = 0;
172 } else if (h_index < object_boxes + 2) {
173 metrics.max_ineq_type = "object_clearance";
174 metrics.max_ineq_index = h_index - 2;
175 } else if (h_index < 2 * object_boxes + 2) {
176 metrics.max_ineq_type = "object_collision";
177 metrics.max_ineq_index = h_index - object_boxes - 2;
178 } else {
179 metrics.max_ineq_type = "vehicle";
180 metrics.max_ineq_index = h_index - 2 * object_boxes - 2;
181 }
182 } else {
183 metrics.max_ineq_type = "slack";
184 metrics.max_ineq_index = index - nb - ng - nh;
185 }
186
187}