30 {
31
32 if (ref_points_.empty()) {
33 AERROR <<
"reference points empty, solver early terminates";
34 return false;
35 }
36
37 if (ref_points_.size() != bounds_around_refs_.size()) {
38 AERROR <<
"ref_points and bounds size not equal, solver early terminates";
39 return false;
40 }
41
42 if (ref_points_.size() < 3) {
43 AERROR <<
"ref_points size smaller than 3, solver early terminates";
44 return false;
45 }
46
47 if (ref_points_.size() > std::numeric_limits<int>::max()) {
48 AERROR <<
"ref_points size too large, solver early terminates";
49 return false;
50 }
51
52
53 num_of_points_ = static_cast<int>(ref_points_.size());
54 num_of_variables_ = num_of_points_ * 2;
55 num_of_constraints_ = num_of_variables_;
56
57
58 std::vector<c_float> P_data;
59 std::vector<c_int> P_indices;
60 std::vector<c_int> P_indptr;
61 CalculateKernel(&P_data, &P_indices, &P_indptr);
62
63
64 std::vector<c_float> A_data;
65 std::vector<c_int> A_indices;
66 std::vector<c_int> A_indptr;
67 std::vector<c_float> lower_bounds;
68 std::vector<c_float> upper_bounds;
69 CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
70 &upper_bounds);
71
72
73 std::vector<c_float> q;
74 CalculateOffset(&q);
75
76
77 std::vector<c_float> primal_warm_start;
78 SetPrimalWarmStart(&primal_warm_start);
79
80 OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
81 OSQPSettings* settings =
82 reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
83
84
85 osqp_set_default_settings(settings);
86 settings->max_iter = max_iter_;
87 settings->time_limit = time_limit_;
88 settings->verbose = verbose_;
89 settings->scaled_termination = scaled_termination_;
90 settings->warm_start = warm_start_;
91
92 OSQPWorkspace* work = nullptr;
93
94 bool res = OptimizeWithOsqp(num_of_variables_, lower_bounds.size(), &P_data,
95 &P_indices, &P_indptr, &A_data, &A_indices,
96 &A_indptr, &lower_bounds, &upper_bounds, &q,
97 &primal_warm_start, data, &work, settings);
98 if (res == false || work == nullptr || work->solution == nullptr) {
99 AERROR <<
"Failed to find solution.";
100
101 osqp_cleanup(work);
102 c_free(data->A);
103 c_free(data->P);
104 c_free(data);
105 c_free(settings);
106
107 return false;
108 }
109
110
111 x_.resize(num_of_points_);
112 y_.resize(num_of_points_);
113 for (int i = 0; i < num_of_points_; ++i) {
114 int index = i * 2;
115 x_.at(i) = work->solution->x[index];
116 y_.at(i) = work->solution->x[index + 1];
117 }
118
119
120 osqp_cleanup(work);
121 c_free(data->A);
122 c_free(data->P);
123 c_free(data);
124 c_free(settings);
125
126 return true;
127}
