-
Notifications
You must be signed in to change notification settings - Fork 10
/
Constrains.cpp
84 lines (69 loc) · 3.01 KB
/
Constrains.cpp
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
//This file is part of ModelPreviewController.
//ModelPreviewController is free software: you can redistribute it and/or modify
//it under the terms of the GNU Lesser General Public License as published by
//the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.
//
//ModelPreviewController is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
//GNU Lesser General Public License for more details.
//
//You should have received a copy of the GNU Lesser General Public License
//along with ModelPreviewController. If not, see <http://www.gnu.org/licenses/>.
#include "Constrains.h"
#include "PreviewSystem.h"
namespace mpc
{
/*************************************************************************************************
* Constrains *
*************************************************************************************************/
Constrain::Constrain(const Eigen::MatrixXd &E, const Eigen::VectorXd &f)
: nrConstr_(0),
E_(E),
A_(),
f_(f),
b_()
{
assert(E_.rows() == f.rows());
}
/*************************************************************************************************
* Trajectory Constrains *
*************************************************************************************************/
void TrajectoryConstrain::initializeConstrain(const PreviewSystem &ps)
{
assert(E_.cols() == ps.xDim);
nrConstr_ = static_cast<int>(E_.rows()) * ps.nrStep;
A_.resize(nrConstr_, ps.fullUDim);
b_.resize(nrConstr_);
}
void TrajectoryConstrain::update(const PreviewSystem &ps)
{
auto nrLines = static_cast<int>(E_.rows());
for (int i = 0; i < ps.nrStep; ++i)
{
A_.block(i * nrLines, 0, nrLines, ps.fullUDim) = E_ * ps.Psi.block(i * ps.xDim, 0, ps.xDim, ps.fullUDim);
b_.segment(i * nrLines, nrLines) = f_ - E_ * (ps.Phi.block(i * ps.xDim, 0, ps.xDim, ps.xDim) * ps.x0 + ps.xi.segment(i * ps.xDim, ps.xDim));
}
}
/*************************************************************************************************
* Constrol Constrains *
*************************************************************************************************/
void ControlConstrain::initializeConstrain(const PreviewSystem &ps)
{
assert(E_.cols() == ps.uDim);
nrConstr_ = static_cast<int>(E_.rows()) * ps.nrStep;
A_.resize(nrConstr_, ps.fullUDim);
b_.resize(nrConstr_);
A_.setZero();
}
void ControlConstrain::update(const PreviewSystem &ps)
{
auto nrLines = static_cast<int>(E_.rows());
for (int i = 0; i < ps.nrStep; ++i)
{
A_.block(i * nrLines, i * ps.uDim, nrLines, ps.uDim) = E_;
b_.segment(i * nrLines, nrLines) = f_;
}
}
} // namespace mpc