solver
Cone.hpp
Go to the documentation of this file.
1 
12 #pragma once
13 
14 #include <memory>
15 #include <vector>
16 #include <iostream>
17 #include <Eigen/Sparse>
18 #include <solver/interface/SolverSetting.hpp>
19 
20 namespace solver {
21 
22  class Cone;
23  class Vector;
24  class ConicVector;
25  class ExtendedVector;
26  class OptimizationVector;
27 
28  class ScalingOperator
29  {
30  public:
31  ScalingOperator(){}
32  ~ScalingOperator(){}
33 
34  void initialize(const Cone& cone) { cone_ = &cone; }
35  ConicVector operator*(const Eigen::Ref<const Eigen::VectorXd>& rhs) const;
36 
37  private:
38  const Cone* cone_;
39  };
40 
46  class NesterovToddScaling
47  {
48  public:
49  friend class Cone;
50 
51  NesterovToddScaling(int conesize);
52  ~NesterovToddScaling(){}
53 
54  inline double& w() { return w_; }
55  inline double& d1() { return d1_; }
56  inline double& u0() { return u0_; }
57  inline double& u1() { return u1_; }
58  inline double& v1() { return v1_; }
59  inline double& eta() { return eta_; }
60  inline double& etaSquare() { return eta_square_; }
61  int& indexSoc(int id) { return SOC_id_(id); }
62  Eigen::VectorXd& scalingSoc() { return wbar_; }
63  double& scalingSoc(int id) { return wbar_(id); }
64 
65  inline const double& w() const { return w_; }
66  inline const double& d1() const { return d1_; }
67  inline const double& u0() const { return u0_; }
68  inline const double& u1() const { return u1_; }
69  inline const double& v1() const { return v1_; }
70  inline const double& eta() const { return eta_; }
71  inline const double& etaSquare() const { return eta_square_; }
72  const int& indexSoc(int id) const { return SOC_id_(id); }
73  const Eigen::VectorXd& scalingSoc() const { return wbar_; }
74  const double& scalingSoc(int id) const { return wbar_(id); }
75 
76  private:
77  Eigen::VectorXd wbar_;
78  Eigen::VectorXi SOC_id_;
79  double w_, d1_, u0_, u1_, v1_, eta_, eta_square_;
80  };
81 
87  class Cone
88  {
89  public:
90  // Cone class
91  Cone(){}
92  ~Cone(){}
93 
94  // Setting up problem
95  void setupLpcone(int size);
96  void setupSocone(const Eigen::VectorXi& indices);
97  void initialize(int nvars, int nleq, int nlineq, const Eigen::VectorXi& nsoc);
98 
99  // Getter and setter methods for problem size variables
100  inline const int numLeq() const { return neq_; }
101  inline const int numSoc() const { return nsoc_; }
102  inline const int sizeSoc() const { return ssoc_; }
103  inline const int sizeLpc() const { return nineq_; }
104  inline const int numVars() const { return nvars_; }
105  inline const int sizeCone() const { return sizecone_; }
106  inline const int extSizeSoc() const { return extssoc_; }
107  inline const int sizeSoc(int id) const { return q_(id); }
108  inline const int numCones() const { return nineq_+nsoc_; }
109  inline const int sizeProb() const { return sizeproblem_; }
110  inline const int extSizeCone() const { return extsizecone_; }
111  inline const int lpConeStart() const { return lpconestart_; }
112  inline const int soConeStart() const { return soconestart_; }
113  inline const int extSizeProb() const { return extsizeproblem_; }
114  inline const int startSoc(int id) const { return SOC_start_(id); }
115  inline const int sizeConstraints() const { return sizeconstraints_; }
116  inline const int optStartSoc(int id) const { return opt_SOC_start_(id); }
117  inline const int extStartSoc(int id) const { return ext_SOC_start_(id); }
118  static double normSoc(const Eigen::Ref<const Eigen::VectorXd>& v) { return v[0]-v.tail(v.size()-1).norm(); }
119 
120  // Getter and setter methods for variables of linear cone
121  int& indexLpc(int id) { return LP_id_(id); }
122  Eigen::VectorXd& scalingLpc() { return LP_scaling_; }
123  double& scalingLpc(int id) { return LP_scaling_(id); }
124  Eigen::VectorXd& sqScalingLpc() { return LP_sq_scaling_; }
125  double& sqScalingLpc(int id) { return LP_sq_scaling_(id); }
126 
127  const int& indexLpc(int id) const { return LP_id_(id); }
128  const Eigen::VectorXd& scalingLpc() const { return LP_scaling_; }
129  const double& scalingLpc(int id) const { return LP_scaling_(id); }
130  const Eigen::VectorXd& sqScalingLpc() const { return LP_sq_scaling_; }
131  const double& sqScalingLpc(int id) const { return LP_sq_scaling_(id); }
132 
133  // Getter and setter methods for variables of second order cone
134  NesterovToddScaling& soc(int id) { return soc_[id]; }
135  const NesterovToddScaling& soc(int id) const { return soc_[id]; }
136  inline double conicResidual(const double* u, int size) const;
137  inline double conicResidual(const double* u, const double* v, int size) const;
138  double conicResidual(const Eigen::Ref<const Eigen::VectorXd>& u) const;
139  double conicResidual(const Eigen::Ref<const Eigen::VectorXd>& u, const Eigen::Ref<const Eigen::VectorXd>& v) const;
140 
141  // Functions for cones
142  double safeDivision(double x, double y) const;
143  void conicProjection(Eigen::Ref<Eigen::VectorXd> s);
144  void conicNTScaling(const double* z, double* lambda) const;
145  void conicNTScaling(const Eigen::VectorXd& z, Eigen::Ref<Eigen::VectorXd> lambda) const;
146  void conicNTScaling2(const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> y) const;
147  void conicDivision(const Eigen::Ref<const Eigen::VectorXd>& u, const Eigen::Ref<const Eigen::VectorXd>& w, Eigen::Ref<Eigen::VectorXd> v) const;
148  double conicProduct(const Eigen::Ref<const Eigen::VectorXd> u, const Eigen::Ref<const Eigen::VectorXd> v, Eigen::Ref<Eigen::VectorXd> w) const;
149  ConeStatus updateNTScalings(const Eigen::VectorXd& s, const Eigen::VectorXd& z, Eigen::VectorXd& lambda);
150  void unpermuteSolution(const Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic>& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd) const;
151  void unpermuteSolution(const Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic>& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd, Eigen::VectorXd& dz) const;
152 
153  // Getter and setter methods for scaling operators
154  ScalingOperator& W() { return W_scaling_operator_; }
155  const ScalingOperator& W() const { return W_scaling_operator_; }
156 
157  private:
158  int nvars_; // number variables
159  int neq_; // number linear equality constraints
160  int nineq_; // number linear inequality constraints
161  int nsoc_; // number second order cone constraints
162  int ssoc_; // size second order cone constraints
163  int extssoc_; // extended size second order cone constraints
164  int lpconestart_; // offset to linear cone start
165  int soconestart_; // offset to second order cone start
166  int sizecone_; // size of linear and second order cones
167  int extsizecone_; // extended size of linear and second order cones
168  int sizeconstraints_; // size of the constraints
169  int sizeproblem_; // size of number of variables plus constraints
170  int extsizeproblem_; // size of number of variables plus constraints
171  Eigen::VectorXi q_; // sizes of second-order cone constraints
172 
173  // variables of LP cone
174  Eigen::VectorXi LP_id_;
175  Eigen::VectorXd LP_scaling_;
176  Eigen::VectorXd LP_sq_scaling_;
177 
178  // variables of SO cone
179  Eigen::VectorXd skbar_, zkbar_;
180  std::vector<NesterovToddScaling> soc_;
181  Eigen::VectorXi SOC_start_, opt_SOC_start_, ext_SOC_start_;
182 
183  // Operators
184  ScalingOperator W_scaling_operator_;
185  };
186 
190  class Vector : public Eigen::VectorXd
191  {
192  public:
193  Vector() : Eigen::VectorXd() {}
194  ~Vector(){}
195 
196  template<typename OtherDerived>
197  Vector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}
198 
199  template<typename OtherDerived>
200  Vector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
201  this->Eigen::VectorXd::operator=(other);
202  return *this;
203  }
204 
205  void initialize(const Cone& cone);
206 
207  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
208  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(), cone_->numLeq()); }
209  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
210  Eigen::Ref<Eigen::VectorXd> yz() { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }
211 
212  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
213  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
214  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }
215 
216  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
217  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(), cone_->numLeq()); }
218  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
219  const Eigen::Ref<const Eigen::VectorXd> yz() const { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }
220 
221  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
222  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
223  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }
224 
225  private:
226  const Cone* cone_;
227  };
228 
232  class ConicVector : public Eigen::VectorXd
233  {
234  public:
235  ConicVector() : Eigen::VectorXd() {}
236  ConicVector(int size) : Eigen::VectorXd(size) {}
237  ~ConicVector(){}
238 
239  template<typename OtherDerived>
240  ConicVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}
241 
242  template<typename OtherDerived>
243  ConicVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
244  this->Eigen::VectorXd::operator=(other);
245  return *this;
246  }
247 
248  ConicVector& operator=(const ConicVector& other) {
249  this->Eigen::VectorXd::operator=(other);
250  this->cone_ = other.cone_;
251  return *this;
252  }
253 
254  void initialize(const Cone& cone);
255  ConicVector operator/ (const ConicVector& rhs) const;
256  ConicVector operator* (const ConicVector& rhs) const;
257  ConicVector operator+ (const ConicVector& rhs) const;
258  ConicVector operator+ (const double& rhs) const;
259  ConicVector operator- (const double& rhs) const;
260  ConicVector& operator+=(const double& rhs);
261  ConicVector operator- () const;
262 
263  Eigen::Ref<Eigen::VectorXd> z() { return this->head(cone_->sizeCone()); }
264  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->head(cone_->sizeLpc()); }
265  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
266  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }
267 
268  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->head(cone_->sizeCone()); }
269  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->head(cone_->sizeLpc()); }
270  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
271  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }
272 
273  private:
274  const Cone* cone_;
275  };
276 
280  class ExtendedVector : public Eigen::VectorXd
281  {
282  public:
283  ExtendedVector() : Eigen::VectorXd() {}
284  ~ExtendedVector(){}
285 
286  template<typename OtherDerived>
287  ExtendedVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}
288 
289  template<typename OtherDerived>
290  ExtendedVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
291  this->Eigen::VectorXd::operator=(other);
292  return *this;
293  }
294 
295  void initialize(const Cone& cone);
296 
297  void zRtoE(const Eigen::Ref<const Eigen::VectorXd>& rvec);
298  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
299  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
300  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }
301 
302  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
303  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
304  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }
305 
306  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
307  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
308  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }
309 
310  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
311  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
312  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }
313 
314  private:
315  const Cone* cone_;
316  };
317 
322  class OptimizationVector : public Eigen::VectorXd
323  {
324  public:
325  OptimizationVector() : Eigen::VectorXd() {}
326  ~OptimizationVector(){}
327 
328  template<typename OtherDerived>
329  OptimizationVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}
330 
331  template<typename OtherDerived>
332  OptimizationVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
333  this->Eigen::VectorXd::operator=(other);
334  this->cone_ = other.cone_;
335  return *this;
336  }
337 
338  void initialize(const Cone& cone);
339 
340  OptimizationVector operator+(const OptimizationVector& rhs) const;
341 
342  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
343  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
344  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
345  Eigen::Ref<Eigen::VectorXd> s() { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
346  Eigen::Ref<Eigen::VectorXd> yz() { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
347  Eigen::Ref<Eigen::VectorXd> yztau() { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
348  double& tau() { return this->segment<2>(cone_->sizeProb())[0]; }
349  double& kappa() { return this->segment<2>(cone_->sizeProb())[1]; }
350 
351  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
352  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
353  const Eigen::Ref<const Eigen::VectorXd> s() const { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
354  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
355  const Eigen::Ref<const Eigen::VectorXd> yz() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
356  const Eigen::Ref<const Eigen::VectorXd> yztau() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
357  double tau() const { return this->segment<2>(cone_->sizeProb())[0]; }
358  double kappa() const { return this->segment<2>(cone_->sizeProb())[1]; }
359 
360  Eigen::Ref<Eigen::VectorXd> xyz() { return this->head(cone_->sizeProb()); }
361  Eigen::Ref<Eigen::VectorXd> xyztau() { return this->head(cone_->sizeProb()+1); }
362  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
363  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
364  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }
365 
366  const Eigen::Ref<const Eigen::VectorXd> xyz() const { return this->head(cone_->sizeProb()); }
367  const Eigen::Ref<const Eigen::VectorXd> xyztau() const { return this->head(cone_->sizeProb()+1); }
368  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
369  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
370  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }
371 
372  private:
373  const Cone* cone_;
374  };
375 
380  class SolverStorage
381  {
382  public:
383  SolverStorage(){}
384  ~SolverStorage(){}
385 
386  double& gTh() { return gTh_; }
387  const double& gTh() const { return gTh_; }
388 
389  Eigen::SparseMatrix<double>& Amatrix() { return A_; }
390  Eigen::SparseMatrix<double>& Gmatrix() { return G_; }
391  Eigen::SparseMatrix<double>& Atmatrix() { return At_; }
392  Eigen::SparseMatrix<double>& Gtmatrix() { return Gt_; }
393  const Eigen::SparseMatrix<double>& Amatrix() const { return A_; }
394  const Eigen::SparseMatrix<double>& Gmatrix() const { return G_; }
395  const Eigen::SparseMatrix<double>& Atmatrix() const { return At_; }
396  const Eigen::SparseMatrix<double>& Gtmatrix() const { return Gt_; }
397 
398  void initializeMatrices();
399  void initialize(Cone& cone, SolverSetting& stgs);
400  void cleanCoeffs() { Acoeffs_.clear(); Gcoeffs_.clear(); }
401  void addCoeff(const Eigen::Triplet<double>& coeff, bool flag_eq = false);
402 
403  Eigen::Ref<Eigen::VectorXd> cbh() { return cbh_; }
404  Eigen::Ref<Eigen::VectorXd> c() { return cbh_.x(); }
405  Eigen::Ref<Eigen::VectorXd> b() { return cbh_.y(); }
406  Eigen::Ref<Eigen::VectorXd> h() { return cbh_.z(); }
407  Eigen::Ref<Eigen::VectorXd> bh() { return cbh_.yz(); }
408  std::vector<Eigen::Triplet<double>>& Acoeffs() { return Acoeffs_; }
409  std::vector<Eigen::Triplet<double>>& Gcoeffs() { return Gcoeffs_; }
410  const Eigen::Ref<const Eigen::VectorXd> cbh() const { return cbh_; }
411  const Eigen::Ref<const Eigen::VectorXd> c() const { return cbh_.x(); }
412  const Eigen::Ref<const Eigen::VectorXd> b() const { return cbh_.y(); }
413  const Eigen::Ref<const Eigen::VectorXd> h() const { return cbh_.z(); }
414  const Eigen::Ref<const Eigen::VectorXd> bh() const { return cbh_.yz(); }
415  const std::vector<Eigen::Triplet<double>>& Acoeffs() const { return Acoeffs_; }
416  const std::vector<Eigen::Triplet<double>>& Gcoeffs() const { return Gcoeffs_; }
417 
418  OptimizationVector& u() { return u_opt_; }
419  OptimizationVector& v() { return v_opt_; }
420  OptimizationVector& ut() { return u_t_opt_; }
421  OptimizationVector& uprev() { return u_prev_opt_; }
422  Vector& cbh_copy() { return cbh_copy_; }
423  const OptimizationVector& u() const { return u_opt_; }
424  const OptimizationVector& v() const { return v_opt_; }
425  const OptimizationVector& ut() const { return u_t_opt_; }
426  const OptimizationVector& uprev() const { return u_prev_opt_; }
427  const Vector& cbh_copy() const { return cbh_copy_; }
428 
429  private:
430  double gTh_;
431  Cone* cone_;
432  SolverSetting* stgs_;
433  Vector cbh_, cbh_copy_;
434  Eigen::SparseMatrix<double> A_, At_, G_, Gt_;
435  std::vector<Eigen::Triplet<double>> Acoeffs_, Gcoeffs_;
436  OptimizationVector u_opt_, v_opt_, u_t_opt_, u_prev_opt_;
437  };
438 
439 }
solver::SolverStorage
Class that provides storage space for the optimization matrices, vectors and variables.
Definition: Cone.hpp:380
solver::OptimizationVector
Helper class to define an optimization vector, including primal and dual variables, and variables to render the optimization problem homogeneous.
Definition: Cone.hpp:322
solver::Vector
Helper class to work with optimization variables, lay down in normal order.
Definition: Cone.hpp:190
solver::SolverSetting
Class that provides access to all environment variables required by the solver.
Definition: SolverSetting.hpp:43
solver
Definition: Cone.hpp:20
solver::NesterovToddScaling
This class performs appropriate scalings of the cone variables.
Definition: Cone.hpp:46
solver::Cone
This class contains all information about the conic optimization problem, and provides functionality ...
Definition: Cone.hpp:87
solver::ConicVector
Helper class to work with variables, which are members of a proper convex cone.
Definition: Cone.hpp:232
solver::ExtendedVector
Helper class to work with optimization variables, lay down in an extended order.
Definition: Cone.hpp:280
solver::ScalingOperator
Definition: Cone.hpp:28
SolverSetting.hpp
ECOS - Embedded Conic Solver.