robotoc
robotoc - efficient ROBOT Optimal Control solvers
Loading...
Searching...
No Matches
split_kkt_matrix.hxx
Go to the documentation of this file.
1#ifndef ROBOTOC_SPLIT_KKT_MATRIX_HXX_
2#define ROBOTOC_SPLIT_KKT_MATRIX_HXX_
3
4#include "robotoc/core/split_kkt_matrix.hpp"
5
6#include <cassert>
7
8
9namespace robotoc {
10
11inline void SplitKKTMatrix::setContactDimension(const int dimf) {
12 assert(dimf >= 0);
13 assert(dimf <= Phit_full_.size());
14 dimf_ = dimf;
15}
16
17
18inline void SplitKKTMatrix::setSwitchingConstraintDimension(const int dims) {
19 assert(dims >= 0);
20 assert(dims <= Phit_full_.size());
21 dims_ = dims;
22}
23
24
25inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Fqq() {
26 return Fxx.topLeftCorner(dimv_, dimv_);
27}
28
29
30inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Fqq() const {
31 return Fxx.topLeftCorner(dimv_, dimv_);
32}
33
34
35inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Fqv() {
36 return Fxx.topRightCorner(dimv_, dimv_);
37}
38
39
40inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Fqv() const {
41 return Fxx.topRightCorner(dimv_, dimv_);
42}
43
44
45inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Fvq() {
46 return Fxx.bottomLeftCorner(dimv_, dimv_);
47}
48
49
50inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Fvq() const {
51 return Fxx.bottomLeftCorner(dimv_, dimv_);
52}
53
54
55inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Fvv() {
56 return Fxx.bottomRightCorner(dimv_, dimv_);
57}
58
59
60inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Fvv() const {
61 return Fxx.bottomRightCorner(dimv_, dimv_);
62}
63
64
65inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::fq() {
66 return fx.head(dimv_);
67}
68
69
70inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::fq() const {
71 return fx.head(dimv_);
72}
73
74
75inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::fv() {
76 return fx.tail(dimv_);
77}
78
79
80inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::fv() const {
81 return fx.tail(dimv_);
82}
83
84
85inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Phix() {
86 return Phix_full_.topLeftCorner(dims_, dimx_);
87}
88
89
90inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Phix() const {
91 return Phix_full_.topLeftCorner(dims_, dimx_);
92}
93
94
95inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Phiq() {
96 return Phix_full_.topLeftCorner(dims_, dimv_);
97}
98
99
100inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Phiq() const {
101 return Phix_full_.topLeftCorner(dims_, dimv_);
102}
103
104
105inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Phiv() {
106 return Phix_full_.topRightCorner(dims_, dimv_);
107}
108
109
110inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Phiv() const {
111 return Phix_full_.topRightCorner(dims_, dimv_);
112}
113
114
115inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Phia() {
116 return Phia_full_.topLeftCorner(dims_, dimv_);
117}
118
119
120inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Phia() const {
121 return Phia_full_.topLeftCorner(dims_, dimv_);
122}
123
124
125inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Phiu() {
126 return Phiu_full_.topLeftCorner(dims_, dimu_);
127}
128
129
130inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Phiu() const {
131 return Phiu_full_.topLeftCorner(dims_, dimu_);
132}
133
134
135inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::Phit() {
136 return Phit_full_.head(dims_);
137}
138
139
140inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::Phit() const {
141 return Phit_full_.head(dims_);
142}
143
144
145inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qqq() {
146 return Qxx.topLeftCorner(dimv_, dimv_);
147}
148
149
150inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qqq() const {
151 return Qxx.topLeftCorner(dimv_, dimv_);
152}
153
154
155inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qqv() {
156 return Qxx.topRightCorner(dimv_, dimv_);
157}
158
159
160inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qqv() const {
161 return Qxx.topRightCorner(dimv_, dimv_);
162}
163
164
165inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qvq() {
166 return Qxx.bottomLeftCorner(dimv_, dimv_);
167}
168
169
170inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qvq() const {
171 return Qxx.bottomLeftCorner(dimv_, dimv_);
172}
173
174
175inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qvv() {
176 return Qxx.bottomRightCorner(dimv_, dimv_);
177}
178
179
180inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qvv() const {
181 return Qxx.bottomRightCorner(dimv_, dimv_);
182}
183
184
185inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qqu() {
186 return Qxu.topLeftCorner(dimv_, dimu_);
187}
188
189
190inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qqu() const {
191 return Qxu.topLeftCorner(dimv_, dimu_);
192}
193
194
195inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qvu() {
196 return Qxu.bottomLeftCorner(dimv_, dimu_);
197}
198
199
200inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qvu() const {
201 return Qxu.bottomLeftCorner(dimv_, dimu_);
202}
203
204
205inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qff() {
206 return Qff_full_.topLeftCorner(dimf_, dimf_);
207}
208
209
210inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qff() const {
211 return Qff_full_.topLeftCorner(dimf_, dimf_);
212}
213
214
215inline Eigen::Block<Eigen::MatrixXd> SplitKKTMatrix::Qqf() {
216 return Qqf_full_.topLeftCorner(dimv_, dimf_);
217}
218
219
220inline const Eigen::Block<const Eigen::MatrixXd> SplitKKTMatrix::Qqf() const {
221 return Qqf_full_.topLeftCorner(dimv_, dimf_);
222}
223
224
225inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::hq() {
226 return hx.head(dimv_);
227}
228
229
230inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::hq() const {
231 return hx.head(dimv_);
232}
233
234
235inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::hv() {
236 return hx.tail(dimv_);
237}
238
239
240inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::hv() const {
241 return hx.tail(dimv_);
242}
243
244
245inline Eigen::VectorBlock<Eigen::VectorXd> SplitKKTMatrix::hf() {
246 return hf_full_.head(dimf_);
247}
248
249
250inline const Eigen::VectorBlock<const Eigen::VectorXd> SplitKKTMatrix::hf() const {
251 return hf_full_.head(dimf_);
252}
253
254
255inline void SplitKKTMatrix::setZero() {
256 Fxx.setZero();
257 Fvu.setZero();
258 fx.setZero();
259 Phix().setZero();
260 Phia().setZero();
261 Phiu().setZero();
262 Phit().setZero();
263 Qxx.setZero();
264 Qaa.setZero();
265 Qdvdv.setZero();
266 Qxu.setZero();
267 Quu.setZero();
268 Qff().setZero();
269 Qqf().setZero();
270 Qtt = 0;
271 Qtt_prev = 0;
272 hx.setZero();
273 hu.setZero();
274 ha.setZero();
275 hf().setZero();
276}
277
278
279inline int SplitKKTMatrix::dimf() const {
280 return dimf_;
281}
282
283
284inline int SplitKKTMatrix::dims() const {
285 return dims_;
286}
287
288} // namespace robotoc
289
290#endif // ROBOTOC_SPLIT_KKT_MATRIX_HXX_
robotoc::SplitKKTMatrix::dimf
int dimf() const
Returns the dimension of the stack of contact forces at the current contact status.
Definition: split_kkt_matrix.hxx:279
robotoc::SplitKKTMatrix::Phiu
Eigen::Block< Eigen::MatrixXd > Phiu()
Jacobian of the swithcing constraint w.r.t. u.
Definition: split_kkt_matrix.hxx:125
robotoc::SplitKKTMatrix::Fqq
Eigen::Block< Eigen::MatrixXd > Fqq()
Jacobian of the state equation (w.r.t. q) w.r.t. q.
Definition: split_kkt_matrix.hxx:25
robotoc::SplitKKTMatrix::Fqv
Eigen::Block< Eigen::MatrixXd > Fqv()
Jacobian of the state equation (w.r.t. q) w.r.t. v.
Definition: split_kkt_matrix.hxx:35
robotoc::SplitKKTMatrix::setContactDimension
void setContactDimension(const int dimf)
Sets contact status, i.e., set dimension of the contact forces.
Definition: split_kkt_matrix.hxx:11
robotoc::SplitKKTMatrix::fx
Eigen::VectorXd fx
Derivative of the discrete time state equation w.r.t. the length of the time interval.
Definition: split_kkt_matrix.hpp:130
robotoc::SplitKKTMatrix::Fvu
Eigen::MatrixXd Fvu
Jacobian of the state equation (w.r.t. v) w.r.t. u.
Definition: split_kkt_matrix.hpp:124
robotoc::SplitKKTMatrix::Qaa
Eigen::MatrixXd Qaa
Hessian w.r.t. the acceleration a and acceleration a.
Definition: split_kkt_matrix.hpp:281
robotoc::SplitKKTMatrix::Phix
Eigen::Block< Eigen::MatrixXd > Phix()
Jacobian of the swithcing constraint w.r.t. x.
Definition: split_kkt_matrix.hxx:85
robotoc::SplitKKTMatrix::Qxu
Eigen::MatrixXd Qxu
Hessian w.r.t. the state x and the control input torques u.
Definition: split_kkt_matrix.hpp:291
robotoc::SplitKKTMatrix::Qqu
Eigen::Block< Eigen::MatrixXd > Qqu()
Hessian of the Lagrangian with respect to the configuration q and control input torques u.
Definition: split_kkt_matrix.hxx:185
robotoc::SplitKKTMatrix::Qff
Eigen::Block< Eigen::MatrixXd > Qff()
Hessian of the Lagrangian with respect to the contact forces f.
Definition: split_kkt_matrix.hxx:205
robotoc::SplitKKTMatrix::hx
Eigen::VectorXd hx
Derivative of the Hamiltonian w.r.t. the state.
Definition: split_kkt_matrix.hpp:361
robotoc::SplitKKTMatrix::Qvv
Eigen::Block< Eigen::MatrixXd > Qvv()
Hessian w.r.t. the joint velocity v and joint velocity v.
Definition: split_kkt_matrix.hxx:175
robotoc::SplitKKTMatrix::Fxx
Eigen::MatrixXd Fxx
Jacobian of the state equation w.r.t. the state x.
Definition: split_kkt_matrix.hpp:71
robotoc::SplitKKTMatrix::fv
Eigen::VectorBlock< Eigen::VectorXd > fv()
Derivative of the discrete-time state equation w.r.t. the velocity v w.r.t. the length of the time in...
Definition: split_kkt_matrix.hxx:75
robotoc::SplitKKTMatrix::Phia
Eigen::Block< Eigen::MatrixXd > Phia()
Jacobian of the swithcing constraint w.r.t. a.
Definition: split_kkt_matrix.hxx:115
robotoc::SplitKKTMatrix::Fvv
Eigen::Block< Eigen::MatrixXd > Fvv()
Jacobian of the state equation (w.r.t. v) to v.
Definition: split_kkt_matrix.hxx:55
robotoc::SplitKKTMatrix::hu
Eigen::VectorXd hu
Derivative of the Hamiltonian w.r.t. the control input.
Definition: split_kkt_matrix.hpp:388
robotoc::SplitKKTMatrix::Phiq
Eigen::Block< Eigen::MatrixXd > Phiq()
Jacobian of the swithcing constraint w.r.t. q.
Definition: split_kkt_matrix.hxx:95
robotoc::SplitKKTMatrix::Qqq
Eigen::Block< Eigen::MatrixXd > Qqq()
Hessian w.r.t. the configuration q and configuration q.
Definition: split_kkt_matrix.hxx:145
robotoc::SplitKKTMatrix::Qvu
Eigen::Block< Eigen::MatrixXd > Qvu()
Hessian of the Lagrangian with respect to the velocity v and control input torques u.
Definition: split_kkt_matrix.hxx:195
robotoc::SplitKKTMatrix::Quu
Eigen::MatrixXd Quu
Hessian w.r.t. the control input torques u and the control input torques u.
Definition: split_kkt_matrix.hpp:321
robotoc::SplitKKTMatrix::Qqf
Eigen::Block< Eigen::MatrixXd > Qqf()
Hessian of the Lagrangian with respect to the configuration and contact forces.
Definition: split_kkt_matrix.hxx:215
robotoc::SplitKKTMatrix::Phit
Eigen::VectorBlock< Eigen::VectorXd > Phit()
Jacobian of the swithcing constraint w.r.t. the switching time.
Definition: split_kkt_matrix.hxx:135
robotoc::SplitKKTMatrix::Qtt
double Qtt
Hessian of the Lagrangian w.r.t. the switching time.
Definition: split_kkt_matrix.hpp:351
robotoc::SplitKKTMatrix::setZero
void setZero()
Set the all components zero.
Definition: split_kkt_matrix.hxx:255
robotoc::SplitKKTMatrix::setSwitchingConstraintDimension
void setSwitchingConstraintDimension(const int dims)
Sets the dimension of the switching constraint.
Definition: split_kkt_matrix.hxx:18
robotoc::SplitKKTMatrix::Qqv
Eigen::Block< Eigen::MatrixXd > Qqv()
Hessian w.r.t. the configuration q and joint velocity v.
Definition: split_kkt_matrix.hxx:155
robotoc::SplitKKTMatrix::hq
Eigen::VectorBlock< Eigen::VectorXd > hq()
Derivative of the Hamiltonian w.r.t. the configuration q.
Definition: split_kkt_matrix.hxx:225
robotoc::SplitKKTMatrix::Fvq
Eigen::Block< Eigen::MatrixXd > Fvq()
Jacobian of the state equation (w.r.t. v) w.r.t. q.
Definition: split_kkt_matrix.hxx:45
robotoc::SplitKKTMatrix::Qdvdv
Eigen::MatrixXd Qdvdv
Hessian w.r.t. the impact change in the velocity ddv.
Definition: split_kkt_matrix.hpp:286
robotoc::SplitKKTMatrix::Phiv
Eigen::Block< Eigen::MatrixXd > Phiv()
Jacobian of the swithcing constraint w.r.t. v.
Definition: split_kkt_matrix.hxx:105
robotoc::SplitKKTMatrix::Qtt_prev
double Qtt_prev
Hessian of the Lagrangian w.r.t. the previoius switching time.
Definition: split_kkt_matrix.hpp:356
robotoc::SplitKKTMatrix::dims
int dims() const
Returns the dimension of the stack of the contact forces at the current contact status.
Definition: split_kkt_matrix.hxx:284
robotoc::SplitKKTMatrix::hv
Eigen::VectorBlock< Eigen::VectorXd > hv()
Derivative of the Hamiltonian w.r.t. the velocity v.
Definition: split_kkt_matrix.hxx:235
robotoc::SplitKKTMatrix::ha
Eigen::VectorXd ha
Derivative of the Hamiltonian w.r.t. the acceleration.
Definition: split_kkt_matrix.hpp:393
robotoc::SplitKKTMatrix::fq
Eigen::VectorBlock< Eigen::VectorXd > fq()
Derivative of the discrete-time state equation w.r.t. the configuration q w.r.t. the length of the ti...
Definition: split_kkt_matrix.hxx:65
robotoc::SplitKKTMatrix::Qxx
Eigen::MatrixXd Qxx
Hessian w.r.t. to the state x and state x.
Definition: split_kkt_matrix.hpp:232
robotoc::SplitKKTMatrix::Qvq
Eigen::Block< Eigen::MatrixXd > Qvq()
Hessian w.r.t. the joint velocity v and configuration q.
Definition: split_kkt_matrix.hxx:165
robotoc::SplitKKTMatrix::hf
Eigen::VectorBlock< Eigen::VectorXd > hf()
Derivative of the Hamiltonian w.r.t. the stack of the contact forces f.
Definition: split_kkt_matrix.hxx:245
robotoc
Definition: constraint_component_base.hpp:17
split_kkt_matrix.hpp