-
Notifications
You must be signed in to change notification settings - Fork 27
/
c3.h
141 lines (124 loc) · 5.5 KB
/
c3.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
#pragma once
#include <vector>
#include <Eigen/Dense>
#include "solvers/c3_options.h"
#include "solvers/fast_osqp_solver.h"
#include "solvers/lcs.h"
#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/osqp_solver.h"
#include "drake/solvers/solve.h"
namespace dairlib {
namespace solvers {
class C3 {
public:
/// @param LCS LCS parameters
/// @param Q, R, G, U Cost function parameters
C3(const LCS& LCS, const std::vector<Eigen::MatrixXd>& Q,
const std::vector<Eigen::MatrixXd>& R,
const std::vector<Eigen::MatrixXd>& G,
const std::vector<Eigen::MatrixXd>& U,
const std::vector<Eigen::VectorXd>& xdesired,
const C3Options& options,
double scaling,
const std::vector<Eigen::VectorXd>& warm_start_delta = {},
const std::vector<Eigen::VectorXd>& warm_start_binary = {},
const std::vector<Eigen::VectorXd>& warm_start_x_ = {},
const std::vector<Eigen::VectorXd>& warm_start_lambda_ = {},
const std::vector<Eigen::VectorXd>& warm_start_u_ = {},
bool warm_start = false);
/// Solve the MPC problem
/// @param x0 The initial state of the system
/// @param delta Copy variable solution
/// @param w Scaled dual variable solution
/// @return The first control action to take, u[0]
Eigen::VectorXd Solve(Eigen::VectorXd& x0,
std::vector<Eigen::VectorXd>& delta,
std::vector<Eigen::VectorXd>& w);
/// Solve a single ADMM step
/// @param x0 The initial state of the system
/// @param delta The copy variables from the previous step
/// @param w The scaled dual variables from the previous step
/// @param G The G variables from previous step
Eigen::VectorXd ADMMStep(Eigen::VectorXd& x0,
std::vector<Eigen::VectorXd>* delta,
std::vector<Eigen::VectorXd>* w,
std::vector<Eigen::MatrixXd>* G);
/// Solve a single QP
/// @param x0 The initial state of the system
/// @param WD The (w - delta) variables
/// @param G The G variables from previous step
std::vector<Eigen::VectorXd> SolveQP(Eigen::VectorXd& x0,
std::vector<Eigen::MatrixXd>& G,
std::vector<Eigen::VectorXd>& WD);
/// Solve the projection problem for all timesteps
/// @param WZ (z + w) variables
/// @param G The G variables from previous step
std::vector<Eigen::VectorXd> SolveProjection(
std::vector<Eigen::MatrixXd>& G, std::vector<Eigen::VectorXd>& WZ, Eigen::VectorXd& x0);
/// allow users to add constraints (adds for all timesteps)
/// @param A, Lowerbound, Upperbound Lowerbound <= A^T x <= Upperbound
/// @param constraint inputconstraint, stateconstraint, forceconstraint
void AddLinearConstraint(Eigen::RowVectorXd& A, double& Lowerbound,
double& Upperbound, int& constraint);
/// allow user to remove all constraints
void RemoveConstraints();
/// Get QP warm start
std::vector<Eigen::VectorXd> GetWarmStartX() const;
std::vector<Eigen::VectorXd> GetWarmStartLambda() const;
std::vector<Eigen::VectorXd> GetWarmStartU() const;
/// Solve a single projection step
/// @param E, F, H, c LCS parameters
/// @param U The U variables
/// @param delta_c The copy of (z + w) variables
virtual Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U,
const Eigen::VectorXd& delta_c,
const Eigen::MatrixXd& E,
const Eigen::MatrixXd& F,
const Eigen::MatrixXd& H,
const Eigen::VectorXd& c,
const int& warm_start_index,
const bool& constrain_first_x,
const Eigen::VectorXd& x0) = 0;
public:
const std::vector<Eigen::MatrixXd> A_;
const std::vector<Eigen::MatrixXd> B_;
const std::vector<Eigen::MatrixXd> D_;
const std::vector<Eigen::VectorXd> d_;
const std::vector<Eigen::MatrixXd> E_;
const std::vector<Eigen::MatrixXd> F_;
const std::vector<Eigen::MatrixXd> H_;
const std::vector<Eigen::VectorXd> c_;
const std::vector<Eigen::MatrixXd> Q_;
const std::vector<Eigen::MatrixXd> R_;
const std::vector<Eigen::MatrixXd> U_;
const std::vector<Eigen::MatrixXd> G_;
const std::vector<Eigen::VectorXd> xdesired_;
const C3Options options_;
const int N_;
const int n_;
const int m_;
const int k_;
const bool hflag_;
double scaling_;
protected:
std::vector<Eigen::VectorXd> warm_start_delta_;
std::vector<Eigen::VectorXd> warm_start_binary_;
std::vector<Eigen::VectorXd> warm_start_x_;
std::vector<Eigen::VectorXd> warm_start_lambda_;
std::vector<Eigen::VectorXd> warm_start_u_;
bool warm_start_;
private:
drake::solvers::MathematicalProgram prog_;
drake::solvers::SolverOptions OSQPoptions_;
drake::solvers::OsqpSolver osqp_;
std::vector<drake::solvers::VectorXDecisionVariable> x_;
std::vector<drake::solvers::VectorXDecisionVariable> u_;
std::vector<drake::solvers::VectorXDecisionVariable> lambda_;
std::vector<drake::solvers::Binding<drake::solvers::QuadraticCost>> costs_;
std::vector<drake::solvers::Binding<drake::solvers::LinearConstraint>>
constraints_;
std::vector<drake::solvers::Binding<drake::solvers::LinearConstraint>>
userconstraints_;
};
} // namespace solvers
} // namespace dairlib