Skip to content

Commit b2b1ea3

Browse files
EdwardOptimizationEdwardOptimization
committed
style: format code with braces
1 parent cc93ee8 commit b2b1ea3

39 files changed

Lines changed: 1258 additions & 809 deletions

‎.clang-format‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,6 @@ ColumnLimit: 100
77
# allow short functions to be written on a single line (suitable for math libraries)
88
AllowShortFunctionsOnASingleLine: Inline
99
# sort include files
10-
SortIncludes: true
10+
SortIncludes: true
11+
# require braces for single-line control statements
12+
InsertBraces: true

‎examples/01_car_tutorial/main.cpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,9 @@ void save_trajectory_csv(const std::string& filename, const SolverType& solver,
3434
std::vector<double> u = solver.get_control(k); // might be empty at N
3535
double g_obs = solver.get_constraint_val(k, 4); // Obstacle constraint
3636

37-
if (k > 0 && k - 1 < static_cast<int>(dts.size()))
37+
if (k > 0 && k - 1 < static_cast<int>(dts.size())) {
3838
current_t += dts[k - 1];
39+
}
3940

4041
// Safe access
4142
double u0 = (k < horizon) ? u[0] : 0.0;
@@ -78,8 +79,9 @@ int main(int /*argc*/, char** /*argv*/)
7879
MiniSolver<CarModel, 50> solver(N, mode, config);
7980

8081
std::vector<double> dts(N);
81-
for (int k = 0; k < N; ++k)
82+
for (int k = 0; k < N; ++k) {
8283
dts[k] = 0.1;
84+
}
8385
solver.set_dt(dts);
8486

8587
// Scenario
@@ -90,8 +92,9 @@ int main(int /*argc*/, char** /*argv*/)
9092
// Initialize Trajectory (Cold Start)
9193
double current_t = 0.0;
9294
for (int k = 0; k <= N; ++k) {
93-
if (k > 0)
95+
if (k > 0) {
9496
current_t += dts[k - 1];
97+
}
9598
double x_ref = current_t * 5.0;
9699

97100
if (k < N) {

‎examples/02_advanced_bicycle/advanced_benchmark.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,9 @@ BenchmarkResult run_test(const std::string& name, SolverConfig config)
5252

5353
std::vector<double> dts(N);
5454
// Simple uniform time steps
55-
for (int k = 0; k < N; ++k)
55+
for (int k = 0; k < N; ++k) {
5656
dts[k] = 0.05;
57+
}
5758

5859
const int NUM_RUNS = 100;
5960
const int WARMUP_RUNS = 10;
@@ -76,8 +77,9 @@ BenchmarkResult run_test(const std::string& name, SolverConfig config)
7677

7778
double current_t = 0.0;
7879
for (int k = 0; k <= N; ++k) {
79-
if (k > 0)
80+
if (k > 0) {
8081
current_t += dts[k - 1];
82+
}
8183
double x_ref = current_t * ExtConfig::TARGET_V;
8284

8385
// Intelligent Reference Generation (Match Debug Setup)
@@ -150,16 +152,18 @@ BenchmarkResult run_test(const std::string& name, SolverConfig config)
150152
for (int i = 0; i < BicycleExtModel::NC; ++i) {
151153
double val
152154
= std::abs(solver.get_constraint_val(k, i) + solver.get_slack(k, i));
153-
if (val > max_viol)
155+
if (val > max_viol) {
154156
max_viol = val;
157+
}
155158
}
156159
}
157160
last_converged
158161
= (status == SolverStatus::OPTIMAL || status == SolverStatus::FEASIBLE);
159162

160163
double cost = 0.0;
161-
for (int k = 0; k <= horizon; ++k)
164+
for (int k = 0; k <= horizon; ++k) {
162165
cost += solver.get_stage_cost(k);
166+
}
163167
last_cost = cost;
164168
last_viol = max_viol;
165169
}

‎examples/02_advanced_bicycle/advanced_debug.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,8 +91,9 @@ int main()
9191
// Setup Problem (Parameters & Initial Guess)
9292
double current_t = 0.0;
9393
for (int k = 0; k <= N; ++k) {
94-
if (k > 0)
94+
if (k > 0) {
9595
current_t += dts[k - 1];
96+
}
9697
double x_ref = current_t * ExtConfig::TARGET_V;
9798

9899
// Smart reference for obstacle avoidance

‎include/minisolver/algorithms/line_search.h‎

Lines changed: 46 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -89,8 +89,9 @@ template <typename Model, int MAX_N> class NoLineSearch : public LineSearchStrat
8989

9090
const double current_dt = (k < N) ? dt_traj[static_cast<size_t>(k)] : 0.0;
9191
detail::evaluate_model_stage<Model>(candidate[k], config, current_dt);
92-
if (k < N)
92+
if (k < N) {
9393
candidate[k + 1].x = candidate[k].f_resid;
94+
}
9495
}
9596
}
9697

@@ -132,10 +133,11 @@ class MeritLineSearch : public LineSearchStrategy<Model, MAX_N> {
132133

133134
// Barrier & Soft Constraint Penalty Calculation
134135
for (int i = 0; i < NC; ++i) {
135-
if (kp.s(i) > config.min_barrier_slack)
136+
if (kp.s(i) > config.min_barrier_slack) {
136137
total_merit -= mu * std::log(kp.s(i));
137-
else
138+
} else {
138139
total_merit += config.barrier_inf_cost;
140+
}
139141

140142
// L1 Soft Constraint: Dual Barrier
141143
double w = 0.0;
@@ -150,15 +152,17 @@ class MeritLineSearch : public LineSearchStrategy<Model, MAX_N> {
150152
// L1 Soft Constraint: Dual Barrier + Linear Penalty
151153
if (type == 1 && w > 1e-6) {
152154
// 1. Barrier terms
153-
if (kp.soft_s(i) > config.min_barrier_slack)
155+
if (kp.soft_s(i) > config.min_barrier_slack) {
154156
total_merit -= mu * std::log(kp.soft_s(i));
155-
else
157+
} else {
156158
total_merit += config.barrier_inf_cost;
159+
}
157160

158-
if (w - kp.lam(i) > 1e-9)
161+
if (w - kp.lam(i) > 1e-9) {
159162
total_merit -= mu * std::log(w - kp.lam(i));
160-
else
163+
} else {
161164
total_merit += config.barrier_inf_cost;
165+
}
162166

163167
// 2. L1 Linear Penalty
164168
total_merit += w * kp.soft_s(i);
@@ -227,12 +231,14 @@ class MeritLineSearch : public LineSearchStrategy<Model, MAX_N> {
227231
double max_dual = 0.0;
228232
for (int k = 0; k <= N; ++k) {
229233
double local_max = MatOps::norm_inf(active[k].lam);
230-
if (local_max > max_dual)
234+
if (local_max > max_dual) {
231235
max_dual = local_max;
236+
}
232237
}
233238
double required_nu = max_dual * 1.1 + 1.0;
234-
if (required_nu > merit_nu)
239+
if (required_nu > merit_nu) {
235240
merit_nu = required_nu;
241+
}
236242

237243
// 2. Initial Merit
238244
double phi_0 = compute_merit(active, N, mu, config);
@@ -280,8 +286,9 @@ class MeritLineSearch : public LineSearchStrategy<Model, MAX_N> {
280286

281287
double current_dt = (k < N) ? dt_traj[k] : 0.0;
282288
detail::evaluate_model_stage<Model>(candidate[k], config, current_dt);
283-
if (k < N)
289+
if (k < N) {
284290
candidate[k + 1].x = candidate[k].f_resid;
291+
}
285292
}
286293
}
287294

@@ -306,8 +313,9 @@ class MeritLineSearch : public LineSearchStrategy<Model, MAX_N> {
306313
}
307314
}
308315

309-
if (accepted)
316+
if (accepted) {
310317
break;
318+
}
311319
alpha *= config.line_search_backtrack_factor;
312320
ls_iter++;
313321
}
@@ -345,10 +353,11 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
345353
// Objective (Phi) Calculation
346354
phi += kp.cost;
347355
for (int i = 0; i < NC; ++i) {
348-
if (kp.s(i) > config.min_barrier_slack)
356+
if (kp.s(i) > config.min_barrier_slack) {
349357
phi -= mu * std::log(kp.s(i));
350-
else
358+
} else {
351359
phi += config.barrier_inf_cost;
360+
}
352361

353362
// L1 Soft Constraint: Dual Barrier
354363
double w = 0.0;
@@ -363,15 +372,17 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
363372
// L1 Soft Constraint
364373
if (type == 1 && w > 1e-6) {
365374
// Barrier terms
366-
if (kp.soft_s(i) > config.min_barrier_slack)
375+
if (kp.soft_s(i) > config.min_barrier_slack) {
367376
phi -= mu * std::log(kp.soft_s(i));
368-
else
377+
} else {
369378
phi += config.barrier_inf_cost;
379+
}
370380

371-
if (w - kp.lam(i) > 1e-9)
381+
if (w - kp.lam(i) > 1e-9) {
372382
phi -= mu * std::log(w - kp.lam(i));
373-
else
383+
} else {
374384
phi += config.barrier_inf_cost;
385+
}
375386

376387
// L1 Linear Penalty
377388
phi += w * kp.soft_s(i);
@@ -434,17 +445,19 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
434445
bool sufficient_decrease = (theta <= (1.0 - config.filter_gamma_theta) * theta_0)
435446
|| (phi <= phi_0 - config.filter_gamma_phi * theta_0);
436447

437-
if (!sufficient_decrease)
448+
if (!sufficient_decrease) {
438449
return false;
450+
}
439451

440452
// Check against filter
441453
for (const auto& entry : filter) {
442454
double theta_j = entry.first;
443455
double phi_j = entry.second;
444456
bool sufficient_wrt_filter = (theta <= (1.0 - config.filter_gamma_theta) * theta_j)
445457
|| (phi <= phi_j - config.filter_gamma_phi * theta_j);
446-
if (!sufficient_wrt_filter)
458+
if (!sufficient_wrt_filter) {
447459
return false;
460+
}
448461
}
449462
return true;
450463
}
@@ -524,8 +537,9 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
524537
double current_dt = (k < N) ? dt_traj[k] : 0.0;
525538

526539
detail::evaluate_model_stage<Model>(candidate[k], config, current_dt);
527-
if (k < N)
540+
if (k < N) {
528541
candidate[k + 1].x = candidate[k].f_resid;
542+
}
529543
}
530544
}
531545

@@ -537,8 +551,9 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
537551
// SOC Logic
538552
if (!accepted && config.enable_soc && !soc_attempted && ls_iter == 0
539553
&& alpha > config.soc_trigger_alpha) {
540-
if (config.print_level >= PrintLevel::DEBUG)
554+
if (config.print_level >= PrintLevel::DEBUG) {
541555
MLOG_DEBUG("Step rejected. Attempting SOC.");
556+
}
542557

543558
// Avoid heap allocation in the solve loop.
544559
TrajArray soc_data = active; // Copy system matrices from active trajectory
@@ -572,30 +587,35 @@ class FilterLineSearch : public LineSearchStrategy<Model, MAX_N> {
572587
double current_dt = (k < N) ? dt_traj[k] : 0.0;
573588
if (config.enable_line_search_rollout) {
574589
// Keep x0 fixed and re-propagate after applying SOC correction.
575-
if (k == 0)
590+
if (k == 0) {
576591
candidate[0].x = active[0].x;
592+
}
577593
}
578594

579595
detail::evaluate_model_stage<Model>(candidate[k], config, current_dt);
580-
if (config.enable_line_search_rollout && k < N)
596+
if (config.enable_line_search_rollout && k < N) {
581597
candidate[k + 1].x = candidate[k].f_resid;
598+
}
582599
}
583600

584601
auto m_soc = compute_metrics(candidate, N, mu, config);
585602
if (is_acceptable(m_soc.first, m_soc.second, theta_0, phi_0, config)) {
586-
if (config.print_level >= PrintLevel::DEBUG)
603+
if (config.print_level >= PrintLevel::DEBUG) {
587604
MLOG_DEBUG("SOC Accepted.");
605+
}
588606
accepted = true;
589607
}
590608
}
591609

592610
soc_attempted = true;
593-
if (accepted)
611+
if (accepted) {
594612
break;
613+
}
595614
}
596615

597-
if (accepted)
616+
if (accepted) {
598617
break;
618+
}
599619
alpha *= config.line_search_backtrack_factor;
600620
ls_iter++;
601621
}

‎include/minisolver/algorithms/model_evaluation.h‎

Lines changed: 11 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -7,19 +7,18 @@
77
namespace minisolver {
88
namespace detail {
99

10-
template <typename Model, typename Knot>
11-
void evaluate_model_stage(Knot& kp, const SolverConfig& config, double dt)
12-
{
13-
if (config.hessian_approximation == HessianApproximation::GAUSS_NEWTON) {
14-
Model::compute_cost_gn(kp);
15-
} else {
16-
Model::compute_cost_exact(kp);
17-
}
10+
template <typename Model, typename Knot>
11+
void evaluate_model_stage(Knot& kp, const SolverConfig& config, double dt)
12+
{
13+
if (config.hessian_approximation == HessianApproximation::GAUSS_NEWTON) {
14+
Model::compute_cost_gn(kp);
15+
} else {
16+
Model::compute_cost_exact(kp);
17+
}
1818

19-
detail::dispatch_compute_dynamics<Model>(
20-
kp, config.integrator, dt, config.newton_config);
21-
Model::compute_constraints(kp);
22-
}
19+
detail::dispatch_compute_dynamics<Model>(kp, config.integrator, dt, config.newton_config);
20+
Model::compute_constraints(kp);
21+
}
2322

2423
} // namespace detail
2524
} // namespace minisolver

‎include/minisolver/algorithms/riccati_solver.h‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,8 +54,9 @@ template <typename TrajArray, typename Model> class RiccatiSolver : public Linea
5454
bool refine(TrajArray& traj, const TrajArray& original_system, int N, double /*mu*/,
5555
double /*reg*/, const SolverConfig& config) override
5656
{
57-
if (!config.enable_iterative_refinement)
57+
if (!config.enable_iterative_refinement) {
5858
return true;
59+
}
5960

6061
// [FIX] Implemented Linear Rollout Refinement (Defect Correction)
6162
// This pass enforces strict dynamic feasibility of the linear solution:
@@ -95,8 +96,9 @@ template <typename TrajArray, typename Model> class RiccatiSolver : public Linea
9596
MSVec<double, Model::NX> error = predicted_dx_next - traj[k + 1].dx;
9697
// [FIX] Use MatOps::norm_inf
9798
double err_norm = MatOps::norm_inf(error);
98-
if (err_norm > max_defect)
99+
if (err_norm > max_defect) {
99100
max_defect = err_norm;
101+
}
100102

101103
// 3. Propagate Correction
102104
// delta_x_{k+1} = A * delta_x + B * delta_u + error

0 commit comments

Comments
 (0)