20std::string timestamp() {
21 const auto now = std::chrono::system_clock::now();
22 const auto time = std::chrono::system_clock::to_time_t(now);
24 gmtime_r(&time, &utc_time);
25 const auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() % 1000;
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';
32int64_t nowNanoseconds() {
33 return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
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");
45 std::error_code error;
46 std::filesystem::create_directories(directory, error);
48 throw std::runtime_error(
"could not create directory '" + directory.string() +
"': " + error.message());
53 throw std::runtime_error(
"could not open '" +
path_.string() +
"'");
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";
75 ocp_nlp_solver* solver,
76 ocp_nlp_config* config,
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);
87 if (!collect_details)
return;
89 ocp_nlp_get(solver,
"res_stat", &metrics.
res_stat);
90 ocp_nlp_get(solver,
"res_comp", &metrics.
res_comp);
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;
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);
116 ocp_nlp_solver* solver,
117 const ocp_nlp_dims* dims,
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) {
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) {
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];
165 }
else if (index < nb + ng) {
167 }
else if (index < nb + ng + nh) {
168 const int h_index = index - nb - ng;
170 metrics.
max_ineq_type = h_index == 0 ?
"boundary_left" :
"boundary_right";
172 }
else if (h_index < object_boxes + 2) {
175 }
else if (h_index < 2 * object_boxes + 2) {
190 ocp_nlp_config* config,
195 bool collect_details) {
199 bool finite_slacks =
true;
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;
223 return finite_slacks;
227 stream_ << std::setprecision(17) << 7 <<
",runtime,," << metrics.
cycle <<
',' << nowNanoseconds() <<
',' << metrics.
ego_stamp_ns
Namespace for trajectory_optimization package.