trajectory_optimization v1.5.0
Loading...
Searching...
No Matches
performance_logger.cpp
Go to the documentation of this file.
1// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
2// SPDX-License-Identifier: Apache-2.0
3
4#include <algorithm>
5#include <chrono>
6#include <cmath>
7#include <cstdlib>
8#include <ctime>
9#include <iomanip>
10#include <sstream>
11#include <stdexcept>
12#include <vector>
13
15
17
18namespace {
19
20std::string timestamp() {
21 const auto now = std::chrono::system_clock::now();
22 const auto time = std::chrono::system_clock::to_time_t(now);
23 std::tm utc_time{};
24 gmtime_r(&time, &utc_time);
25 const auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() % 1000;
26
27 std::ostringstream value;
28 value << std::put_time(&utc_time, "%Y%m%dT%H%M%S") << '_' << std::setfill('0') << std::setw(3) << milliseconds << 'Z';
29 return value.str();
30}
31
32int64_t nowNanoseconds() {
33 return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
34}
35
36} // namespace
37
38PerformanceLogger::PerformanceLogger(const std::string& node_name) {
39 const char* configured_directory = std::getenv("TRAJECTORY_OPTIMIZATION_BENCHMARK_DIR");
40 const std::filesystem::path directory = configured_directory != nullptr && *configured_directory != '\0'
41 ? std::filesystem::path(configured_directory)
42 : std::filesystem::temp_directory_path() / "trajectory_optimization_benchmarks";
43 path_ = directory / (node_name + '_' + timestamp() + ".csv");
44
45 std::error_code error;
46 std::filesystem::create_directories(directory, error);
47 if (error) {
48 throw std::runtime_error("could not create directory '" + directory.string() + "': " + error.message());
49 }
50
51 stream_.open(path_, std::ios::out | std::ios::trunc);
52 if (!stream_) {
53 throw std::runtime_error("could not open '" + path_.string() + "'");
54 }
56 << "schema_version,source,run_id,cycle,record_stamp_ns,ego_stamp_ns,reference_stamp_ns,route_stamp_ns,status,"
57 "published,ref_points,objects,relaxed_attempted,failure_phase,relaxed_status,sqp_iter,qp_iter,"
58 "qp_status,cycle_ms,preprocessing_ms,solve_wall_ms,relaxed_solve_wall_ms,constrained_solve_wall_ms,postprocessing_ms,"
59 "acados_total_ms,acados_lin_ms,"
60 "acados_sim_ms,acados_qp_ms,"
61 "acados_qp_solver_ms,acados_qp_xcond_ms,acados_reg_ms,acados_glob_ms,acados_preparation_ms,"
62 "acados_feedback_ms,cost,kkt,nlp_res,res_stat,res_eq,res_ineq,res_comp,"
63 "max_ineq_violation,max_ineq_stage,max_ineq_type,max_ineq_index,max_ineq_side,"
64 "max_eq_violation,max_eq_stage,max_eq_state,"
65 "max_object_clearance_slack,max_object_clearance_slack_stage,max_object_clearance_slack_index\n";
66 stream_.flush();
67}
68
73
75 ocp_nlp_solver* solver,
76 ocp_nlp_config* config,
77 ocp_nlp_dims* dims,
78 ocp_nlp_in* input,
79 ocp_nlp_out* output,
80 bool collect_details) {
81 ocp_nlp_eval_cost(solver, input, output);
82 ocp_nlp_eval_residuals(solver, input, output);
83 ocp_nlp_get(solver, "cost_value", &metrics.cost_value);
84 ocp_nlp_get(solver, "res_eq", &metrics.res_eq);
85 ocp_nlp_get(solver, "res_ineq", &metrics.res_ineq);
86
87 if (!collect_details) return;
88
89 ocp_nlp_get(solver, "res_stat", &metrics.res_stat);
90 ocp_nlp_get(solver, "res_comp", &metrics.res_comp);
91 metrics.nlp_res = std::max({metrics.res_stat, metrics.res_eq, metrics.res_ineq, metrics.res_comp});
92
93 auto readTime = [&](const char* field, double& destination_ms) {
94 double time_seconds = 0.0;
95 ocp_nlp_get(solver, field, &time_seconds);
96 destination_ms = time_seconds * 1000.0;
97 };
98
99 readTime("time_tot", metrics.acados_total_ms);
100 readTime("time_lin", metrics.acados_lin_ms);
101 readTime("time_sim", metrics.acados_sim_ms);
102 readTime("time_qp", metrics.acados_qp_ms);
103 readTime("time_qp_solver_call", metrics.acados_qp_solver_ms);
104 readTime("time_qp_xcond", metrics.acados_qp_xcond_ms);
105 readTime("time_reg", metrics.acados_reg_ms);
106 readTime("time_glob", metrics.acados_glob_ms);
107 readTime("time_preparation", metrics.acados_preparation_ms);
108 readTime("time_feedback", metrics.acados_feedback_ms);
109 ocp_nlp_get(solver, "sqp_iter", &metrics.sqp_iter);
110 ocp_nlp_get(solver, "qp_iter", &metrics.qp_iter);
111 ocp_nlp_get(solver, "qp_status", &metrics.qp_status);
112 ocp_nlp_out_get(config, dims, output, 0, "kkt_norm_inf", &metrics.kkt_norm_inf);
113}
114
116 ocp_nlp_solver* solver,
117 const ocp_nlp_dims* dims,
118 int object_boxes) {
119 // The acados C API exposes stage-dependent dimensions as raw arrays.
120 // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
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 // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
187}
188
190 ocp_nlp_config* config,
191 ocp_nlp_dims* dims,
192 ocp_nlp_in* input,
193 ocp_nlp_out* output,
194 int object_boxes,
195 bool collect_details) {
196 metrics.max_object_clearance_slack = 0.0;
199 bool finite_slacks = true;
200 // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
201 for (int stage = 0; stage <= dims->N; ++stage) {
202 const int ns = dims->ns[stage];
203 if (ns == 0) continue;
204 std::vector<double> lower(ns), upper(ns);
205 ocp_nlp_out_get(config, dims, output, stage, "sl", lower.data());
206 ocp_nlp_out_get(config, dims, output, stage, "su", upper.data());
207 finite_slacks = finite_slacks && std::all_of(lower.begin(), lower.end(), [](double value) { return std::isfinite(value); }) &&
208 std::all_of(upper.begin(), upper.end(), [](double value) { return std::isfinite(value); });
209 if (!collect_details) continue;
210 std::vector<int> slack_indices(ns);
211 ocp_nlp_constraints_model_get(config, dims, input, stage, "idxs", slack_indices.data());
212 const int clearance_start = dims->nb[stage] + dims->ng[stage] + 2;
213 for (int slack = 0; slack < ns; ++slack) {
214 const int object_index = slack_indices[slack] - clearance_start;
215 if (object_index >= 0 && object_index < object_boxes && lower[slack] > metrics.max_object_clearance_slack) {
216 metrics.max_object_clearance_slack = lower[slack];
217 metrics.max_object_clearance_slack_stage = stage;
218 metrics.max_object_clearance_slack_index = object_index;
219 }
220 }
221 }
222 // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
223 return finite_slacks;
224}
225
227 stream_ << std::setprecision(17) << 7 << ",runtime,," << metrics.cycle << ',' << nowNanoseconds() << ',' << metrics.ego_stamp_ns
228 << ',' << metrics.reference_stamp_ns << ',' << metrics.route_stamp_ns << ',' << metrics.status << ','
229 << (metrics.published ? 1 : 0) << ',' << metrics.reference_points << ',' << metrics.objects << ','
230 << (metrics.relaxed_attempted ? 1 : 0) << ',' << metrics.failure_phase << ',' << metrics.relaxed_status << ','
231 << metrics.sqp_iter << ',' << metrics.qp_iter << ',' << metrics.qp_status << ',' << metrics.cycle_ms << ','
232 << metrics.preprocessing_ms << ',' << metrics.solve_wall_ms << ',' << metrics.relaxed_solve_wall_ms << ','
233 << metrics.constrained_solve_wall_ms << ',' << metrics.postprocessing_ms << ',' << metrics.acados_total_ms << ','
234 << metrics.acados_lin_ms << ',' << metrics.acados_sim_ms << ',' << metrics.acados_qp_ms << ','
235 << metrics.acados_qp_solver_ms << ',' << metrics.acados_qp_xcond_ms << ',' << metrics.acados_reg_ms << ','
236 << metrics.acados_glob_ms << ',' << metrics.acados_preparation_ms << ',' << metrics.acados_feedback_ms << ','
237 << metrics.cost_value << ',' << metrics.kkt_norm_inf << ',' << metrics.nlp_res << ',' << metrics.res_stat << ','
238 << metrics.res_eq << ',' << metrics.res_ineq << ',' << metrics.res_comp << ',' << metrics.max_ineq_violation << ','
239 << metrics.max_ineq_stage << ',' << metrics.max_ineq_type << ',' << metrics.max_ineq_index << ','
240 << metrics.max_ineq_side << ',' << metrics.max_eq_violation << ',' << metrics.max_eq_stage << ','
241 << metrics.max_eq_state << ',' << metrics.max_object_clearance_slack << ',' << metrics.max_object_clearance_slack_stage
242 << ',' << metrics.max_object_clearance_slack_index << '\n';
243
245 stream_.flush();
247 }
248}
249
250} // namespace trajectory_optimization
PerformanceLogger(const std::string &node_name)
Creates a CSV performance log for the given node.
static void collectConstraintDiagnostics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, const ocp_nlp_dims *dims, int object_boxes)
Collects the largest equality and inequality constraint violations from acados.
void write(const PerformanceMetrics &metrics)
Appends one set of performance metrics to the CSV log.
static bool collectSlackStatistics(PerformanceMetrics &metrics, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, int object_boxes, bool collect_details)
Checks all slacks for finiteness and optionally collects desired-clearance statistics.
~PerformanceLogger()
Flushes and closes the performance log.
static void collectSolverStatistics(PerformanceMetrics &metrics, ocp_nlp_solver *solver, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, bool collect_details)
Reads timing, iteration, cost, and residual statistics from acados.
Namespace for trajectory_optimization package.