- 3.0.2 optimal control module.
ConstraintContainerAD-impl.h
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4  **********************************************************************************************************************/
5 
6 #pragma once
7 
8 #ifdef CPPADCG
9 
10 namespace ct {
11 namespace optcon {
12 
13 
14 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
15 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD()
16 {
17  stateControlD_.setZero();
18 
19  fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
20  return this->evaluateIntermediateCodegen(stateinput);
21  };
22 
23  fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
24  return this->evaluateTerminalCodegen(stateinput);
25  };
26 
27  intermediateCodegen_ =
28  std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
29  terminalCodegen_ =
30  std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
31 }
32 
33 
34 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
35 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const state_vector_t& x,
36  const input_vector_t& u,
37  const SCALAR& t)
38 {
39  //Set to some random number which is != the initguess of the problem
40  stateControlD_ << x, u;
41 }
42 
43 
44 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
45 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const ConstraintContainerAD& arg)
46  : LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
47  fIntermediate_(arg.fIntermediate_),
48  fTerminal_(arg.fTerminal_),
49  sparsityIntermediateRows_(arg.sparsityIntermediateRows_),
50  sparsityStateIntermediateRows_(arg.sparsityStateIntermediateRows_),
51  sparsityStateIntermediateCols_(arg.sparsityStateIntermediateCols_),
52  sparsityInputIntermediateRows_(arg.sparsityInputIntermediateRows_),
53  sparsityInputIntermediateCols_(arg.sparsityInputIntermediateCols_),
54  sparsityTerminalRows_(arg.sparsityTerminalRows_),
55  sparsityStateTerminalRows_(arg.sparsityStateTerminalRows_),
56  sparsityStateTerminalCols_(arg.sparsityStateTerminalCols_),
57  sparsityInputTerminalRows_(arg.sparsityInputTerminalRows_),
58  sparsityInputTerminalCols_(arg.sparsityInputTerminalCols_),
59  stateControlD_(arg.stateControlD_)
60 {
61  constraintsIntermediate_.resize(arg.constraintsIntermediate_.size());
62  constraintsTerminal_.resize(arg.constraintsTerminal_.size());
63 
64  for (size_t i = 0; i < constraintsIntermediate_.size(); ++i)
65  constraintsIntermediate_[i] =
66  std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsIntermediate_[i]->clone());
67 
68  for (size_t i = 0; i < constraintsTerminal_.size(); ++i)
69  constraintsTerminal_[i] =
70  std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsTerminal_[i]->clone());
71 
72  intermediateCodegen_ =
73  std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
74  terminalCodegen_ =
75  std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
76 
77  // make sure libraries get compiled in clone
78  initializeIntermediate();
79  initializeTerminal();
80 }
81 
82 
83 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
84 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD_Raw_Ptr_t
85 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
86 {
87  return new ConstraintContainerAD(*this);
88 }
89 
90 
91 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
92 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerAD()
93 {
94 }
95 
96 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
97 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateConstraint(
98  std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
99  bool verbose)
100 {
101  constraintsIntermediate_.push_back(constraint);
102  if (verbose)
103  {
104  std::string name;
105  constraint->getName(name);
106  std::cout << "''" << name << "'' added as AD intermediate constraint " << std::endl;
107  }
108 
109  fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
110  return this->evaluateIntermediateCodegen(stateinput);
111  };
112 
113  intermediateCodegen_->update(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount());
114 
115  this->initializedIntermediate_ = false;
116 }
117 
118 
119 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
120 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addTerminalConstraint(
121  std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
122  bool verbose)
123 {
124  constraintsTerminal_.push_back(constraint);
125  if (verbose)
126  {
127  std::string name;
128  constraint->getName(name);
129  std::cout << "''" << name << "'' added as AD terminal constraint " << std::endl;
130  }
131 
132  fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
133  return this->evaluateTerminalCodegen(stateinput);
134  };
135 
136  terminalCodegen_->update(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount());
137 
138  this->initializedTerminal_ = false;
139 }
140 
141 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
142 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
143 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
144 {
145  if (!this->initializedIntermediate_)
146  throw std::runtime_error("evaluateIntermediateConstraints not initialized yet. Call 'initialize()' before");
147 
148  return intermediateCodegen_->forwardZero(stateControlD_);
149 }
150 
151 
152 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
153 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
154 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
155 {
156  if (!this->initializedTerminal_)
157  throw std::runtime_error("evaluateTerminalConstraints not initialized yet. Call 'initialize()' before");
158 
159  return terminalCodegen_->forwardZero(stateControlD_);
160 }
161 
162 
163 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
164 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateConstraintsCount()
165 {
166  size_t count = 0;
167 
168  for (auto constraint : constraintsIntermediate_)
169  count += constraint->getConstraintSize();
170 
171  return count;
172 }
173 
174 
175 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
176 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getTerminalConstraintsCount()
177 {
178  size_t count = 0;
179  for (auto constraint : constraintsTerminal_)
180  count += constraint->getConstraintSize();
181 
182  return count;
183 }
184 
185 
186 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
187 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
188 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseIntermediate()
189 {
190  if (!this->initializedIntermediate_)
191  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
192 
193  MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
194 
195  // std::cout << "jacTot" << std::endl;
196 
197  VectorXs jacSparse;
198  jacSparse.resize(getJacobianStateNonZeroCountIntermediate());
199  for (size_t i = 0; i < getJacobianStateNonZeroCountIntermediate(); ++i)
200  jacSparse(i) = (jacTot.template leftCols<STATE_DIM>())(
201  sparsityStateIntermediateRows_(i), sparsityStateIntermediateCols_(i));
202 
203  return jacSparse;
204 }
205 
206 
207 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
208 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
209 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateIntermediate()
210 {
211  if (!this->initializedIntermediate_)
212  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
213 
214  MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
215  return jacTot.template leftCols<STATE_DIM>();
216 }
217 
218 
219 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
220 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
221 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseTerminal()
222 {
223  if (!this->initializedTerminal_)
224  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
225 
226  MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
227  VectorXs jacSparse;
228  jacSparse.resize(getJacobianStateNonZeroCountTerminal());
229  for (size_t i = 0; i < getJacobianStateNonZeroCountTerminal(); ++i)
230  jacSparse(i) =
231  (jacTot.template leftCols<STATE_DIM>())(sparsityStateTerminalRows_(i), sparsityStateTerminalCols_(i));
232 
233  return jacSparse;
234 }
235 
236 
237 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
238 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
239 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateTerminal()
240 {
241  if (!this->initializedTerminal_)
242  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
243 
244  MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
245  return jacTot.template leftCols<STATE_DIM>();
246 }
247 
248 
249 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
250 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
251 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseIntermediate()
252 {
253  MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
254  VectorXs jacSparse;
255  jacSparse.resize(getJacobianInputNonZeroCountIntermediate());
256  for (size_t i = 0; i < getJacobianInputNonZeroCountIntermediate(); ++i)
257  jacSparse(i) = (jacTot.template rightCols<CONTROL_DIM>())(
258  sparsityInputIntermediateRows_(i), sparsityInputIntermediateCols_(i));
259 
260  return jacSparse;
261 }
262 
263 
264 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
265 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
266 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputIntermediate()
267 {
268  if (!this->initializedIntermediate_)
269  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
270 
271  MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
272  return jacTot.template rightCols<CONTROL_DIM>();
273 }
274 
275 
276 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
277 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
278 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseTerminal()
279 {
280  if (!this->initializedTerminal_)
281  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
282 
283  MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
284  VectorXs jacSparse;
285  jacSparse.resize(getJacobianInputNonZeroCountTerminal());
286  for (size_t i = 0; i < getJacobianInputNonZeroCountTerminal(); ++i)
287  jacSparse(i) =
288  (jacTot.template rightCols<CONTROL_DIM>())(sparsityInputTerminalRows_(i), sparsityInputTerminalCols_(i));
289 
290  return jacSparse;
291 }
292 
293 
294 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
295 typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
296 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputTerminal()
297 {
298  if (!this->initializedTerminal_)
299  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
300 
301  MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
302  return jacTot.template rightCols<CONTROL_DIM>();
303 }
304 
305 
306 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
307 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateIntermediate(Eigen::VectorXi& iRows,
308  Eigen::VectorXi& jCols)
309 {
310  if (!this->initializedIntermediate_)
311  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
312 
313  iRows = sparsityStateIntermediateRows_;
314  jCols = sparsityStateIntermediateCols_;
315 }
316 
317 
318 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
319 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateTerminal(Eigen::VectorXi& iRows,
320  Eigen::VectorXi& jCols)
321 {
322  if (!this->initializedTerminal_)
323  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
324 
325  iRows = sparsityStateTerminalRows_;
326  jCols = sparsityStateTerminalCols_;
327 }
328 
329 
330 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
331 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputIntermediate(Eigen::VectorXi& iRows,
332  Eigen::VectorXi& jCols)
333 {
334  if (!this->initializedIntermediate_)
335  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
336 
337  iRows = sparsityInputIntermediateRows_;
338  jCols = sparsityInputIntermediateCols_;
339 }
340 
341 
342 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
343 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputTerminal(Eigen::VectorXi& iRows,
344  Eigen::VectorXi& jCols)
345 {
346  if (!this->initializedTerminal_)
347  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
348 
349  iRows = sparsityInputTerminalRows_;
350  jCols = sparsityInputTerminalCols_;
351 }
352 
353 
354 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
355 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountIntermediate()
356 {
357  if (!this->initializedIntermediate_)
358  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
359 
360  return sparsityStateIntermediateRows_.rows();
361 }
362 
363 
364 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
365 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountTerminal()
366 {
367  if (!this->initializedTerminal_)
368  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
369 
370  return sparsityStateTerminalRows_.rows();
371 }
372 
373 
374 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
375 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountIntermediate()
376 {
377  if (!this->initializedIntermediate_)
378  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
379 
380  return sparsityInputIntermediateRows_.rows();
381 }
382 
383 
384 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
385 size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountTerminal()
386 {
387  if (!this->initializedTerminal_)
388  throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
389 
390  return sparsityInputTerminalRows_.rows();
391 }
392 
393 
394 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
395 bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeIntermediate()
396 {
397  Eigen::VectorXi sparsityRows;
398  Eigen::VectorXi sparsityCols;
399 
400  if (getIntermediateConstraintsCount() > 0)
401  {
402  ct::core::DerivativesCppadSettings settings;
403  settings.createForwardZero_ = true;
404  settings.createJacobian_ = true;
405  settings.createSparseJacobian_ = true;
406 
407  intermediateCodegen_->compileJIT(settings, "intermediateConstraints");
408  intermediateCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
409 
410  std::cout << "sparsityPattern Intermediate: " << std::endl
411  << intermediateCodegen_->getSparsityPatternJacobian() << std::endl;
412  assert(sparsityRows.rows() == sparsityRows.rows());
413 
414  int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
415  int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
416 
417  sparsityStateIntermediateRows_.resize(nonZerosState);
418  sparsityStateIntermediateCols_.resize(nonZerosState);
419  sparsityInputIntermediateRows_.resize(nonZerosInput);
420  sparsityInputIntermediateCols_.resize(nonZerosInput);
421 
422  size_t count = 0;
423 
424  this->lowerBoundsIntermediate_.resize(getIntermediateConstraintsCount());
425  this->upperBoundsIntermediate_.resize(getIntermediateConstraintsCount());
426 
427  for (auto constraint : constraintsIntermediate_)
428  {
429  size_t constraintSize = constraint->getConstraintSize();
430  this->lowerBoundsIntermediate_.segment(count, constraintSize) = constraint->getLowerBound();
431  this->upperBoundsIntermediate_.segment(count, constraintSize) = constraint->getUpperBound();
432  count += constraintSize;
433  }
434 
435  size_t stateIndex = 0;
436  size_t inputIndex = 0;
437 
438  for (int i = 0; i < sparsityRows.rows(); ++i)
439  {
440  if (sparsityCols(i) < static_cast<int>(STATE_DIM))
441  {
442  sparsityStateIntermediateRows_(stateIndex) = sparsityRows(i);
443  sparsityStateIntermediateCols_(stateIndex) = sparsityCols(i);
444  stateIndex++;
445  }
446  else
447  {
448  sparsityInputIntermediateRows_(inputIndex) = sparsityRows(i);
449  sparsityInputIntermediateCols_(inputIndex) = sparsityCols(i) - STATE_DIM;
450  inputIndex++;
451  }
452  }
453  }
454 
455  return true;
456 }
457 
458 
459 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
460 bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeTerminal()
461 {
462  Eigen::VectorXi sparsityRows;
463  Eigen::VectorXi sparsityCols;
464 
465  if (getTerminalConstraintsCount() > 0)
466  {
467  ct::core::DerivativesCppadSettings settings;
468  settings.createForwardZero_ = true;
469  settings.createJacobian_ = true;
470  settings.createSparseJacobian_ = true;
471 
472  terminalCodegen_->compileJIT(settings, "terminalConstraints");
473  terminalCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
474 
475  std::cout << "sparsityPattern Terminal: " << std::endl
476  << terminalCodegen_->getSparsityPatternJacobian() << std::endl;
477  assert(sparsityRows.rows() == sparsityRows.rows());
478 
479  int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
480  int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
481 
482  sparsityStateTerminalRows_.resize(nonZerosState);
483  sparsityStateTerminalCols_.resize(nonZerosState);
484  sparsityInputTerminalRows_.resize(nonZerosInput);
485  sparsityInputTerminalCols_.resize(nonZerosInput);
486 
487  size_t count = 0;
488 
489  this->lowerBoundsTerminal_.resize(getTerminalConstraintsCount());
490  this->upperBoundsTerminal_.resize(getTerminalConstraintsCount());
491 
492  for (auto constraint : constraintsTerminal_)
493  {
494  size_t constraintSize = constraint->getConstraintSize();
495  this->lowerBoundsTerminal_.segment(count, constraintSize) = constraint->getLowerBound();
496  this->upperBoundsTerminal_.segment(count, constraintSize) = constraint->getUpperBound();
497  count += constraintSize;
498  }
499 
500  size_t stateIndex = 0;
501  size_t inputIndex = 0;
502 
503  for (int i = 0; i < sparsityRows.rows(); ++i)
504  {
505  if (sparsityCols(i) < static_cast<int>(STATE_DIM))
506  {
507  sparsityStateTerminalRows_(stateIndex) = sparsityRows(i);
508  sparsityStateTerminalCols_(stateIndex) = sparsityCols(i);
509  stateIndex++;
510  }
511  else
512  {
513  sparsityInputTerminalRows_(inputIndex) = sparsityRows(i);
514  sparsityInputTerminalCols_(inputIndex) = sparsityCols(i) - static_cast<int>(STATE_DIM);
515  inputIndex++;
516  }
517  }
518  }
519 
520  return true;
521 }
522 
523 
524 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
525 void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::update()
526 {
527  stateControlD_ << this->x_, this->u_;
528 }
529 
530 
531 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
532 Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
533 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateCodegen(
534  const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
535 {
536  size_t count = 0;
537  Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
538 
539  for (auto constraint : constraintsIntermediate_)
540  {
541  size_t constraint_dim = constraint->getConstraintSize();
542  gLocal.conservativeResize(count + constraint_dim);
543  gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
544  stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
545  count += constraint_dim;
546  }
547  return gLocal;
548 }
549 
550 
551 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
552 Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
553 ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalCodegen(
554  const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
555 {
556  size_t count = 0;
557  Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
558 
559  for (auto constraint : constraintsTerminal_)
560  {
561  size_t constraint_dim = constraint->getConstraintSize();
562  gLocal.conservativeResize(count + constraint_dim);
563  gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
564  stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
565  count += constraint_dim;
566  }
567 
568  return gLocal;
569 }
570 
571 } // namespace optcon
572 } // namespace ct
573 
574 #endif
u
ct::core::ControlVector< control_dim > u
Definition: LoadFromFileTest.cpp:21
t
clear all close all load ct GNMSLog0 mat reformat t
Definition: gnmsPlot.m:6
SCALAR
CppAD::AD< CppAD::cg::CG< double > > SCALAR
i
for i
Definition: mpc_unittest_plotting.m:14
JacCG
ct::core::DerivativesCppadCG< state_dim, control_dim > JacCG
x
ct::core::StateVector< state_dim > x
Definition: LoadFromFileTest.cpp:20
state_vector_t
Eigen::Matrix< double, nStates, 1 > state_vector_t
ct::optcon::example::verbose
const bool verbose
Definition: ConstraintComparison.h:18