/
solver_primal_dual.h
371 lines (301 loc) · 14.2 KB
/
solver_primal_dual.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
#pragma once
#include <iostream>
#include <Eigen/SparseLU>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
#include "least_square_problem.h"
#include "residuals.h"
#include "solver_sparse.h"
#include "types.h"
#include "visualizer.h"
// Inherient SparseSolver for code reuse and tests
class RocketLandingSolver_PrimalDualInteriorPoint : public SparseSolver {
// Note: primal variables is embeded in problem.trajectory
// Need to sleep, don't want to refactor it.
using PrimalVariables = Trajectory;
public:
void solver_rocket_landing_constrained_least_squares(const Config& config,
RocketLandingProblem& problem)
{
config_ = config;
num_states_ = problem.trajectory.states.size();
num_primal_variables_ = problem.trajectory.states.size() * RocketState::STATE_SIZE;
num_dual_variables_ = problem.constrains.linear_constrains.size();
const int augmented_var_size = num_primal_variables_ + num_dual_variables_;
// avoid memory reallocation
NormalEquation augmented_normal_equ(augmented_var_size);
NormalEquation normal_equ(num_primal_variables_);
VectorXd dual_vars = initialize_dual_variables();
assert(dual_vars.size() == num_dual_variables_);
// print_variables(problem.trajectory, true);
constexpr int MAX_ITERATIONS = 200;
int iter = 0;
for (; iter < MAX_ITERATIONS; ++iter) {
ScopeProfiler p("solve_iter");
problem.update_problem();
// unconstrained problem
residual_function_to_normal_equation(problem.residuals, normal_equ);
// compute the gradient of the cost function
VectorXd cost_grad = compute_cost_gradient(normal_equ, problem.trajectory, problem.residuals);
apply_regularization_to_hessian(problem.residuals, normal_equ);
// Primal dual problem
// Solve (Stationarity, Complementary slackness) of KKT
const double k_relax_complementary_slackness = compute_k_relax_complementary_slackness(iter, problem, dual_vars);
construct_primal_dual_problem(normal_equ.lhs, cost_grad,
dual_vars, problem.constrains, k_relax_complementary_slackness, augmented_normal_equ);
// solve linear system
VectorXd delta_primal_dual = solve_normal_equation(augmented_normal_equ);
assert(delta_primal_dual.size() == num_primal_variables_ + num_dual_variables_);
// Enforce interior point
// (primal feasibility + dual feasibility) of KKT
bool status = back_tracking_line_search_and_update(delta_primal_dual, problem, dual_vars);
if (status == false) {
std::cerr << "update failed at iter: " << iter << std::endl;
std::cout << "update failed at iter: " << iter << std::endl;
break;
}
if (false) {
print_variables(problem.trajectory);
PRINT_NAME_VAR(problem.residuals.total_cost());
PRINT_NAME_VAR(problem.residuals.prior_cost());
PRINT_NAME_VAR(problem.residuals.motion_cost());
PRINT_NAME_VAR(problem.residuals.regularization_cost());
}
if (stop_condition(augmented_normal_equ.rhs, problem.constrains, dual_vars) == true) {
std::cout << "stop at iteration:" << iter << std::endl;
break;
}
}
if (iter == MAX_ITERATIONS) {
std::cout << "warning: not converged" << std::endl;
}
if(verbose_)
print_variables(problem.trajectory, true);
}
protected:
double compute_k_relax_complementary_slackness(const size_t iteration,
const RocketLandingProblem& problem,
const VectorXd& dual_variables) const
{
// Trick
// Using log barrier for some iterations
// Smaller t => smoother cost
if (iteration < 5) {
return 1.;
}
const double surrogate_duality_gap = compute_surrogate_duality_gap(problem.constrains, dual_variables);
const double k_relax_complementary_slackness = primal_dual_mu_ * num_dual_variables_ / surrogate_duality_gap;
// PRINT_NAME_VAR(k_relax_complementary_slackness);
return k_relax_complementary_slackness;
}
VectorXd initialize_dual_variables()
{
return 1e1 * VectorXd::Ones(num_dual_variables_);
}
VectorXd compute_cost_gradient(const NormalEquation& normal_equ,
const PrimalVariables& trajectory,
const RocketLandingResiduals& residuals)
{
const VectorXd primal_vars = trajectory_to_vectorXd(trajectory);
// cost = ||r||^2 + ||x||^2_w (W is ignored for residual)
// cost gradient = 2 * Dr * r + wx, Dr * r = A.T * b
VectorXd cost_gradient = -2. * normal_equ.rhs;
assert(cost_gradient.rows() == num_primal_variables_);
auto set_regularization_func = [&](const double regularization,
const int reg_idx) {
for (int state_i = 0; state_i < residuals.num_rocket_states; ++state_i) {
const int var_idx = state_i * RocketState::STATE_SIZE + reg_idx;
cost_gradient(var_idx) += 2 * regularization * primal_vars(var_idx);
}
};
if (residuals.time_regularization > 0) {
set_regularization_func(residuals.time_regularization, RocketState::i_dt);
}
if (residuals.acceleration_regularization > 0) {
set_regularization_func(residuals.acceleration_regularization, RocketState::i_acceleration);
}
if (residuals.turning_rate_regularization > 0) {
set_regularization_func(residuals.turning_rate_regularization, RocketState::i_turning_rate);
}
return cost_gradient;
}
// Solve for KKT: (Stationarity, Complementary slackness)
virtual void construct_primal_dual_problem(const MatrixXd& cost_hessian,
const VectorXd& cost_gradient,
const VectorXd& dual_variables,
const Constrains& constrains,
const double k_relax_complementary_slackness,
NormalEquation& augmented_normal_equ)
{
ScopeProfiler p("construct_primal_dual_problem");
construct_primal_dual_problem_lhs(
cost_hessian, dual_variables, constrains, augmented_normal_equ.lhs);
augmented_normal_equ.rhs.setZero();
augmented_normal_equ.rhs = construct_primal_dual_problem_rhs(
cost_gradient, dual_variables, constrains, k_relax_complementary_slackness);
if (false)
PythonmatplotVisualizer().spy_matrix(augmented_normal_equ.lhs);
}
// Solve for KKT: (Stationarity, Complementary slackness)
void construct_primal_dual_problem_lhs(const MatrixXd& cost_hessian,
const VectorXd& dual_variables,
const Constrains& constrains,
MatrixXd& augmented_normal_equ_lhs)
{
const int num_variables = cost_hessian.rows();
augmented_normal_equ_lhs.setZero();
// Copy primal equation
augmented_normal_equ_lhs.block(0, 0, num_variables, num_variables) = cost_hessian;
// Not general, simplified for 1d linear case.
for (size_t constrain_idx = 0; constrain_idx < constrains.linear_constrains.size(); ++constrain_idx) {
const auto& constrain_linear = constrains.linear_constrains[constrain_idx];
const int correspond_primal_index = constrain_linear.state_index * RocketState::STATE_SIZE
+ constrain_linear.type_index;
const int dual_var_index = num_variables + constrain_idx;
// upper left
// hessian for linear constrain is 0. Don't need to update it
// upper right block
augmented_normal_equ_lhs(correspond_primal_index, dual_var_index) = constrain_linear.jacobian();
// lower left block
augmented_normal_equ_lhs(dual_var_index, correspond_primal_index)
= -dual_variables[constrain_idx] * constrain_linear.jacobian();
// lower right block
augmented_normal_equ_lhs(dual_var_index, dual_var_index) = -constrain_linear.h();
}
}
VectorXd construct_primal_dual_problem_rhs(const VectorXd& cost_gradient,
const VectorXd& dual_variables,
const Constrains& constrains,
const double k_relax_complementary_slackness)
{
const int num_variables = cost_gradient.rows();
const int num_constrains = constrains.linear_constrains.size();
const int augmented_var_size = num_variables + num_constrains;
VectorXd relaxed_kkt_rhs = VectorXd::Zero(augmented_var_size);
// relaxed_kkt_rhs.block(0, 0, num_variables, 1) = normal_equ.rhs;
relaxed_kkt_rhs.segment(0, num_variables) = -cost_gradient;
// Not general, simplified for 1d linear case.
for (size_t constrain_idx = 0; constrain_idx < constrains.linear_constrains.size(); ++constrain_idx) {
const auto& constrain_linear = constrains.linear_constrains[constrain_idx];
const int correspond_primal_index = constrain_linear.state_index * RocketState::STATE_SIZE
+ constrain_linear.type_index;
const int dual_var_index = num_variables + constrain_idx;
// Dual residual: KKT Stationarity
relaxed_kkt_rhs(correspond_primal_index) -= dual_variables[constrain_idx] * constrain_linear.jacobian();
// Cent residual: KKT relaxed complementary slackness
relaxed_kkt_rhs(dual_var_index) -= -dual_variables[constrain_idx] * constrain_linear.h()
- 1. / k_relax_complementary_slackness;
}
return relaxed_kkt_rhs;
}
bool dual_feasible(const VectorXd& dual_variables,
const VectorXd& dual_step,
const double step_size)
{
const VectorXd updated_dual_var = dual_variables + step_size * dual_step;
// u_i > 0 for all i,
if ((updated_dual_var.array() >= 0.).all()) {
return true;
} else {
return false;
}
}
bool primal_feasible(Constrains& constrains,
const PrimalVariables& trajectory)
{
return constrains.trajectory_satisfy_constrains(trajectory);
}
bool back_tracking_line_search_and_update(const VectorXd& delta_primal_dual,
RocketLandingProblem& problem,
VectorXd& dual_variables)
{
const double current_cost = problem.residuals.total_cost();
double updated_cost = std::numeric_limits<double>::max();
auto check_cost_deacrease = [&](const PrimalVariables& trajectory) {
const RocketLandingResiduals updated_residuals = compute_residual(trajectory,
problem.start_state, config_.weight_start, problem.end_state, config_.weight_end, num_states_);
// TODO: I should use primal-dual cost here. But it is expensive.
updated_cost = updated_residuals.total_cost();
// Back tracking for value decrase
if (updated_cost < current_cost) {
return true;
} else {
return false;
}
};
double final_line_search_step = 0.;
bool search_success = false;
// Cache
bool dual_feasible_status = false;
bool primal_feasible_status = false;
const VectorXd primal_step = delta_primal_dual.segment(0, num_primal_variables_);
const VectorXd dual_step = delta_primal_dual.segment(num_primal_variables_, num_dual_variables_);
for (double step_size = 1.; step_size > 1e-2; step_size *= back_tracking_scale_) {
if (dual_feasible_status == false) {
if (dual_feasible(dual_variables, dual_step, step_size) == false) {
continue;
} else {
dual_feasible_status = true;
}
}
PrimalVariables updated_primal_vars = update_primal_variables(primal_step, step_size, problem.trajectory);
// TODO: update check_cost_deacrease to use primal-dual cost
final_line_search_step = step_size;
if (primal_feasible_status == false) {
if (primal_feasible(problem.constrains, updated_primal_vars) == false) {
continue;
} else {
primal_feasible_status = true;
}
}
if (check_cost_deacrease(updated_primal_vars) == false) {
continue;
} else {
search_success = true;
final_line_search_step = step_size;
break;
}
}
// update dual & primal vars
dual_variables += final_line_search_step * dual_step;
problem.trajectory = update_primal_variables(primal_step, final_line_search_step, problem.trajectory);
if (verbose_)
{
PRINT_NAME_VAR(search_success);
PRINT_NAME_VAR(final_line_search_step);
}
return true;
}
double compute_surrogate_duality_gap(const Constrains& constrains,
const VectorXd& dual_variables) const
{
double gap_value = 0.;
for (size_t constrain_idx = 0; constrain_idx < constrains.linear_constrains.size(); ++constrain_idx) {
const LinearConstrain1D& constrain_linear = constrains.linear_constrains[constrain_idx];
gap_value += -dual_variables[constrain_idx] * constrain_linear.h();
}
return gap_value;
}
// Simple stop condition
bool stop_condition(const VectorXd& residual,
const Constrains& constrains,
const VectorXd& dual_variables)
{
const double primal_dual_cost = float(residual.transpose() * residual) / num_states_;
const double surrogate_duality_gap = compute_surrogate_duality_gap(constrains, dual_variables);
if (primal_dual_cost > 1e-4) {
return false;
}
if (surrogate_duality_gap > 1e-4) {
return false;
}
return true;
}
Config config_;
// Config
double back_tracking_scale_ = { 0.5 };
double primal_dual_mu_ = 10.;
int num_states_ = { -1 };
int num_primal_variables_ = { -1 };
int num_dual_variables_ = { -1 };
};