33 {
34
35 if (ref_points_.empty()) {
36 AERROR <<
"reference points empty, solver early terminates";
37 return false;
38 }
39
40 if (ref_points_.size() != bounds_around_refs_.size()) {
41 AERROR <<
"ref_points and bounds size not equal, solver early terminates";
42 return false;
43 }
44
45 if (ref_points_.size() < 3) {
46 AERROR <<
"ref_points size smaller than 3, solver early terminates";
47 return false;
48 }
49
50 if (ref_points_.size() > std::numeric_limits<int>::max()) {
51 AERROR <<
"ref_points size too large, solver early terminates";
52 return false;
53 }
54
55
56 num_of_points_ = static_cast<int>(ref_points_.size());
57 num_of_pos_variables_ = num_of_points_ * 2;
58 num_of_slack_variables_ = num_of_points_ - 2;
59 num_of_variables_ = num_of_pos_variables_ + num_of_slack_variables_;
60
61 num_of_variable_constraints_ = num_of_variables_;
62 num_of_curvature_constraints_ = num_of_points_ - 2;
63 num_of_constraints_ =
64 num_of_variable_constraints_ + num_of_curvature_constraints_;
65
66
67 std::vector<c_float> primal_warm_start;
68 SetPrimalWarmStart(ref_points_, &primal_warm_start);
69
70
71 std::vector<c_float> P_data;
72 std::vector<c_int> P_indices;
73 std::vector<c_int> P_indptr;
74 CalculateKernel(&P_data, &P_indices, &P_indptr);
75
76
77 std::vector<c_float> q;
78 CalculateOffset(ref_points_, &q);
79
80
81 std::vector<c_float> A_data;
82 std::vector<c_int> A_indices;
83 std::vector<c_int> A_indptr;
84 std::vector<c_float> lower_bounds;
85 std::vector<c_float> upper_bounds;
86 CalculateAffineConstraint(ref_points_, &A_data, &A_indices, &A_indptr,
87 &lower_bounds, &upper_bounds);
88
89
90 OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
91 data->n = num_of_variables_;
92 data->m = num_of_constraints_;
93 data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
94 P_indices.data(), P_indptr.data());
95 data->q = q.data();
96 data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
97 A_indices.data(), A_indptr.data());
98 data->l = lower_bounds.data();
99 data->u = upper_bounds.data();
100
101
102 OSQPSettings* settings =
103 reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
104 osqp_set_default_settings(settings);
105 settings->max_iter = max_iter_;
106 settings->time_limit = time_limit_;
107 settings->verbose = verbose_;
108 settings->scaled_termination = scaled_termination_;
109 settings->warm_start = warm_start_;
110 settings->polish = true;
111 settings->eps_abs = 1e-5;
112 settings->eps_rel = 1e-5;
113 settings->eps_prim_inf = 1e-5;
114 settings->eps_dual_inf = 1e-5;
115
116
117 OSQPWorkspace* work = nullptr;
118
119 work = osqp_setup(data, settings);
120
121
122 bool initial_solve_res = OptimizeWithOsqp(primal_warm_start, &work);
123
124 if (!initial_solve_res) {
125 AERROR <<
"initial iteration solving fails";
126 osqp_cleanup(work);
127 c_free(data->A);
128 c_free(data->P);
129 c_free(data);
130 c_free(settings);
131 return false;
132 }
133
134
135
136 int pen_itr = 0;
137 double ctol = 0.0;
138 double original_slack_penalty = weight_curvature_constraint_slack_var_;
139 double last_fvalue = work->info->obj_val;
140
141 while (pen_itr < sqp_pen_max_iter_) {
142 int sub_itr = 1;
143 bool fconverged = false;
144
145 while (sub_itr < sqp_sub_max_iter_) {
146 SetPrimalWarmStart(opt_xy_, &primal_warm_start);
147 CalculateOffset(opt_xy_, &q);
148 CalculateAffineConstraint(opt_xy_, &A_data, &A_indices, &A_indptr,
149 &lower_bounds, &upper_bounds);
150 osqp_update_lin_cost(work, q.data());
151 osqp_update_A(work, A_data.data(), OSQP_NULL, A_data.size());
152 osqp_update_bounds(work, lower_bounds.data(), upper_bounds.data());
153
154 bool iterative_solve_res = OptimizeWithOsqp(primal_warm_start, &work);
155 if (!iterative_solve_res) {
156 AERROR <<
"iteration at " << sub_itr
157 << ", solving fails with max sub iter " << sqp_sub_max_iter_;
158 weight_curvature_constraint_slack_var_ = original_slack_penalty;
159 osqp_cleanup(work);
160 c_free(data->A);
161 c_free(data->P);
162 c_free(data);
163 c_free(settings);
164 return false;
165 }
166
167 double cur_fvalue = work->info->obj_val;
168 double ftol = std::abs((last_fvalue - cur_fvalue) / last_fvalue);
169
170 if (ftol < sqp_ftol_) {
171 ADEBUG <<
"merit function value converges at sub itr num " << sub_itr;
172 ADEBUG <<
"merit function value converges to " << cur_fvalue
173 << ", with ftol " << ftol << ", under max_ftol " << sqp_ftol_;
174 fconverged = true;
175 break;
176 }
177
178 last_fvalue = cur_fvalue;
179 ++sub_itr;
180 }
181
182 if (!fconverged) {
183 AERROR <<
"Max number of iteration reached";
184 weight_curvature_constraint_slack_var_ = original_slack_penalty;
185 osqp_cleanup(work);
186 c_free(data->A);
187 c_free(data->P);
188 c_free(data);
189 c_free(settings);
190 return false;
191 }
192
193 ctol = CalculateConstraintViolation(opt_xy_);
194
195 ADEBUG <<
"ctol is " << ctol <<
", at pen itr " << pen_itr;
196
197 if (ctol < sqp_ctol_) {
198 ADEBUG <<
"constraint satisfied at pen itr num " << pen_itr;
199 ADEBUG <<
"constraint voilation value drops to " << ctol
200 << ", under max_ctol " << sqp_ctol_;
201 weight_curvature_constraint_slack_var_ = original_slack_penalty;
202 osqp_cleanup(work);
203 c_free(data->A);
204 c_free(data->P);
205 c_free(data);
206 c_free(settings);
207 return true;
208 }
209
210 weight_curvature_constraint_slack_var_ *= 10;
211 ++pen_itr;
212 }
213
214 ADEBUG <<
"constraint not satisfied with total itr num " << pen_itr;
215 ADEBUG <<
"constraint voilation value drops to " << ctol
216 << ", higher than max_ctol " << sqp_ctol_;
217 weight_curvature_constraint_slack_var_ = original_slack_penalty;
218 osqp_cleanup(work);
219 c_free(data->A);
220 c_free(data->P);
221 c_free(data);
222 c_free(settings);
223 return true;
224}