Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
new file mode 100644
index 0000000..3668114
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
@@ -0,0 +1,571 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD()
+{
+ stateControlD_.setZero();
+
+ fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateIntermediateCodegen(stateinput);
+ };
+
+ fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateTerminalCodegen(stateinput);
+ };
+
+ intermediateCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
+ terminalCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR& t)
+{
+ //Set to some random number which is != the initguess of the problem
+ stateControlD_ << x, u;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const ConstraintContainerAD& arg)
+ : LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ fIntermediate_(arg.fIntermediate_),
+ fTerminal_(arg.fTerminal_),
+ sparsityIntermediateRows_(arg.sparsityIntermediateRows_),
+ sparsityStateIntermediateRows_(arg.sparsityStateIntermediateRows_),
+ sparsityStateIntermediateCols_(arg.sparsityStateIntermediateCols_),
+ sparsityInputIntermediateRows_(arg.sparsityInputIntermediateRows_),
+ sparsityInputIntermediateCols_(arg.sparsityInputIntermediateCols_),
+ sparsityTerminalRows_(arg.sparsityTerminalRows_),
+ sparsityStateTerminalRows_(arg.sparsityStateTerminalRows_),
+ sparsityStateTerminalCols_(arg.sparsityStateTerminalCols_),
+ sparsityInputTerminalRows_(arg.sparsityInputTerminalRows_),
+ sparsityInputTerminalCols_(arg.sparsityInputTerminalCols_),
+ stateControlD_(arg.stateControlD_)
+{
+ constraintsIntermediate_.resize(arg.constraintsIntermediate_.size());
+ constraintsTerminal_.resize(arg.constraintsTerminal_.size());
+
+ for (size_t i = 0; i < constraintsIntermediate_.size(); ++i)
+ constraintsIntermediate_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsIntermediate_[i]->clone());
+
+ for (size_t i = 0; i < constraintsTerminal_.size(); ++i)
+ constraintsTerminal_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsTerminal_[i]->clone());
+
+ intermediateCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
+ terminalCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
+
+ // make sure libraries get compiled in clone
+ initializeIntermediate();
+ initializeTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD_Raw_Ptr_t
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ConstraintContainerAD(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerAD()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsIntermediate_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as AD intermediate constraint " << std::endl;
+ }
+
+ fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateIntermediateCodegen(stateinput);
+ };
+
+ intermediateCodegen_->update(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount());
+
+ this->initializedIntermediate_ = false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addTerminalConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsTerminal_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as AD terminal constraint " << std::endl;
+ }
+
+ fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateTerminalCodegen(stateinput);
+ };
+
+ terminalCodegen_->update(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount());
+
+ this->initializedTerminal_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("evaluateIntermediateConstraints not initialized yet. Call 'initialize()' before");
+
+ return intermediateCodegen_->forwardZero(stateControlD_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("evaluateTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ return terminalCodegen_->forwardZero(stateControlD_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateConstraintsCount()
+{
+ size_t count = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getTerminalConstraintsCount()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+
+ // std::cout << "jacTot" << std::endl;
+
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianStateNonZeroCountIntermediate());
+ for (size_t i = 0; i < getJacobianStateNonZeroCountIntermediate(); ++i)
+ jacSparse(i) = (jacTot.template leftCols<STATE_DIM>())(
+ sparsityStateIntermediateRows_(i), sparsityStateIntermediateCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ return jacTot.template leftCols<STATE_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianStateNonZeroCountTerminal());
+ for (size_t i = 0; i < getJacobianStateNonZeroCountTerminal(); ++i)
+ jacSparse(i) =
+ (jacTot.template leftCols<STATE_DIM>())(sparsityStateTerminalRows_(i), sparsityStateTerminalCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ return jacTot.template leftCols<STATE_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseIntermediate()
+{
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianInputNonZeroCountIntermediate());
+ for (size_t i = 0; i < getJacobianInputNonZeroCountIntermediate(); ++i)
+ jacSparse(i) = (jacTot.template rightCols<CONTROL_DIM>())(
+ sparsityInputIntermediateRows_(i), sparsityInputIntermediateCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ return jacTot.template rightCols<CONTROL_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianInputNonZeroCountTerminal());
+ for (size_t i = 0; i < getJacobianInputNonZeroCountTerminal(); ++i)
+ jacSparse(i) =
+ (jacTot.template rightCols<CONTROL_DIM>())(sparsityInputTerminalRows_(i), sparsityInputTerminalCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ return jacTot.template rightCols<CONTROL_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateIntermediate(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityStateIntermediateRows_;
+ jCols = sparsityStateIntermediateCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityStateTerminalRows_;
+ jCols = sparsityStateTerminalCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputIntermediate(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityInputIntermediateRows_;
+ jCols = sparsityInputIntermediateCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityInputTerminalRows_;
+ jCols = sparsityInputTerminalCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityStateIntermediateRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityStateTerminalRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityInputIntermediateRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityInputTerminalRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeIntermediate()
+{
+ Eigen::VectorXi sparsityRows;
+ Eigen::VectorXi sparsityCols;
+
+ if (getIntermediateConstraintsCount() > 0)
+ {
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createSparseJacobian_ = true;
+
+ intermediateCodegen_->compileJIT(settings, "intermediateConstraints");
+ intermediateCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
+
+ std::cout << "sparsityPattern Intermediate: " << std::endl
+ << intermediateCodegen_->getSparsityPatternJacobian() << std::endl;
+ assert(sparsityRows.rows() == sparsityRows.rows());
+
+ int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
+ int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
+
+ sparsityStateIntermediateRows_.resize(nonZerosState);
+ sparsityStateIntermediateCols_.resize(nonZerosState);
+ sparsityInputIntermediateRows_.resize(nonZerosInput);
+ sparsityInputIntermediateCols_.resize(nonZerosInput);
+
+ size_t count = 0;
+
+ this->lowerBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->upperBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsIntermediate_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsIntermediate_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+
+ size_t stateIndex = 0;
+ size_t inputIndex = 0;
+
+ for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ {
+ if (sparsityCols(i) < STATE_DIM)
+ {
+ sparsityStateIntermediateRows_(stateIndex) = sparsityRows(i);
+ sparsityStateIntermediateCols_(stateIndex) = sparsityCols(i);
+ stateIndex++;
+ }
+ else
+ {
+ sparsityInputIntermediateRows_(inputIndex) = sparsityRows(i);
+ sparsityInputIntermediateCols_(inputIndex) = sparsityCols(i) - STATE_DIM;
+ inputIndex++;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeTerminal()
+{
+ Eigen::VectorXi sparsityRows;
+ Eigen::VectorXi sparsityCols;
+
+ if (getTerminalConstraintsCount() > 0)
+ {
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createSparseJacobian_ = true;
+
+ terminalCodegen_->compileJIT(settings, "terminalConstraints");
+ terminalCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
+
+ std::cout << "sparsityPattern Terminal: " << std::endl
+ << terminalCodegen_->getSparsityPatternJacobian() << std::endl;
+ assert(sparsityRows.rows() == sparsityRows.rows());
+
+ int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
+ int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
+
+ sparsityStateTerminalRows_.resize(nonZerosState);
+ sparsityStateTerminalCols_.resize(nonZerosState);
+ sparsityInputTerminalRows_.resize(nonZerosInput);
+ sparsityInputTerminalCols_.resize(nonZerosInput);
+
+ size_t count = 0;
+
+ this->lowerBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->upperBoundsTerminal_.resize(getTerminalConstraintsCount());
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsTerminal_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsTerminal_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+
+ size_t stateIndex = 0;
+ size_t inputIndex = 0;
+
+ for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ {
+ if (sparsityCols(i) < STATE_DIM)
+ {
+ sparsityStateTerminalRows_(stateIndex) = sparsityRows(i);
+ sparsityStateTerminalCols_(stateIndex) = sparsityCols(i);
+ stateIndex++;
+ }
+ else
+ {
+ sparsityInputTerminalRows_(inputIndex) = sparsityRows(i);
+ sparsityInputTerminalCols_(inputIndex) = sparsityCols(i) - STATE_DIM;
+ inputIndex++;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::update()
+{
+ stateControlD_ << this->x_, this->u_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
+{
+ size_t count = 0;
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ gLocal.conservativeResize(count + constraint_dim);
+ gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
+ stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
+ count += constraint_dim;
+ }
+ return gLocal;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
+{
+ size_t count = 0;
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ gLocal.conservativeResize(count + constraint_dim);
+ gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
+ stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
+ count += constraint_dim;
+ }
+
+ return gLocal;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h
new file mode 100644
index 0000000..022b7af
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h
@@ -0,0 +1,188 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LinearConstraintContainer.h"
+#include "term/ConstraintBase.h"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Contains all the constraints using with AD generated jacobians
+ *
+ * @tparam STATE_DIM { description }
+ * @tparam CONTROL_DIM { description }
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerAD : public LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::DerivativesCppadJIT<STATE_DIM + CONTROL_DIM, -1> JacCG;
+ typedef typename JacCG::CG_SCALAR CGScalar;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintContainerAD_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+
+ /**
+ * @brief Basic constructor
+ */
+ ConstraintContainerAD();
+
+ /**
+ * \brief Constructor using state, control and time
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ ConstraintContainerAD(const state_vector_t& x, const input_vector_t& u, const SCALAR& t = 0.0);
+
+
+ /**
+ * \brief Copy constructor
+ * @param arg constraint class to copy
+ */
+ ConstraintContainerAD(const ConstraintContainerAD& arg);
+
+ /**
+ * Deep-cloning of Constraint
+ *
+ * @return base pointer to the clone
+ */
+ virtual ConstraintContainerAD_Raw_Ptr_t clone() const override;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintContainerAD();
+
+ /**
+ * @brief Adds an intermediate constraint.
+ *
+ * @param[in] constraint The constraint
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addIntermediateConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ /**
+ * @brief Adds a terminal constraint.
+ *
+ * @param[in] constraint The constraint
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addTerminalConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ virtual VectorXs evaluateIntermediate() override;
+
+ virtual VectorXs evaluateTerminal() override;
+
+ virtual size_t getIntermediateConstraintsCount() override;
+
+ virtual size_t getTerminalConstraintsCount() override;
+
+ virtual VectorXs jacobianStateSparseIntermediate() override;
+
+ virtual MatrixXs jacobianStateIntermediate() override;
+
+ virtual VectorXs jacobianStateSparseTerminal() override;
+
+ virtual MatrixXs jacobianStateTerminal() override;
+
+ virtual VectorXs jacobianInputSparseIntermediate() override;
+
+ virtual MatrixXs jacobianInputIntermediate() override;
+
+ virtual VectorXs jacobianInputSparseTerminal() override;
+
+ virtual MatrixXs jacobianInputTerminal() override;
+
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual size_t getJacobianStateNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianStateNonZeroCountTerminal() override;
+
+ virtual size_t getJacobianInputNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianInputNonZeroCountTerminal() override;
+
+ virtual bool initializeIntermediate() override;
+
+ virtual bool initializeTerminal() override;
+
+private:
+ // cache the matrix here, no need to call it at every eval matrix
+ virtual void update() override;
+
+ /**
+ * @brief Helper function to keep track of the constraint evaluation
+ * used for cppad codegeneration
+ *
+ * @param[in] stateinput The stacked state input vector
+ *
+ * @return The evaluated intermediate constraints
+ */
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> evaluateIntermediateCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput);
+
+ /**
+ * @brief Helper function to keep track of the constraint evaluation
+ * used for cppad codegeneration
+ *
+ * @param[in] stateinput The stacked state input vector
+ *
+ * @return The evaluated terminal constraints
+ */
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> evaluateTerminalCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput);
+
+ //containers
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsIntermediate_;
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsTerminal_;
+
+ std::shared_ptr<JacCG> intermediateCodegen_;
+ std::shared_ptr<JacCG> terminalCodegen_;
+
+ typename JacCG::FUN_TYPE_CG fIntermediate_;
+ typename JacCG::FUN_TYPE_CG fTerminal_;
+
+ Eigen::VectorXi sparsityIntermediateRows_;
+ Eigen::VectorXi sparsityStateIntermediateRows_;
+ Eigen::VectorXi sparsityStateIntermediateCols_;
+ Eigen::VectorXi sparsityInputIntermediateRows_;
+ Eigen::VectorXi sparsityInputIntermediateCols_;
+
+ Eigen::VectorXi sparsityTerminalRows_;
+ Eigen::VectorXi sparsityStateTerminalRows_;
+ Eigen::VectorXi sparsityStateTerminalCols_;
+ Eigen::VectorXi sparsityInputTerminalRows_;
+ Eigen::VectorXi sparsityInputTerminalCols_;
+
+
+ Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM, 1> stateControlD_; /** contains x, u in stacked form */
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h
new file mode 100644
index 0000000..c747332
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h
@@ -0,0 +1,590 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR& t)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical(
+ const ConstraintContainerAnalytical& arg)
+ : LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ constraintsIntermediate_(arg.constraintsIntermediate_),
+ constraintsTerminal_(arg.constraintsTerminal_),
+ evalIntermediate_(arg.evalIntermediate_),
+ evalJacSparseStateIntermediate_(arg.evalJacSparseStateIntermediate_),
+ evalJacSparseInputIntermediate_(arg.evalJacSparseInputIntermediate_),
+ evalJacDenseStateIntermediate_(arg.evalJacDenseStateIntermediate_),
+ evalJacDenseInputIntermediate_(arg.evalJacDenseInputIntermediate_),
+ evalTerminal_(arg.evalTerminal_),
+ evalJacSparseStateTerminal_(arg.evalJacSparseStateTerminal_),
+ evalJacSparseInputTerminal_(arg.evalJacSparseInputTerminal_),
+ evalJacDenseStateTerminal_(arg.evalJacDenseStateTerminal_),
+ evalJacDenseInputTerminal_(arg.evalJacDenseInputTerminal_)
+{
+ // vectors of terms can be resized easily
+ constraintsIntermediate_.resize(arg.constraintsIntermediate_.size());
+ constraintsTerminal_.resize(arg.constraintsTerminal_.size());
+
+ for (size_t i = 0; i < constraintsIntermediate_.size(); ++i)
+ constraintsIntermediate_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsIntermediate_[i]->clone());
+
+ for (size_t i = 0; i < constraintsTerminal_.size(); ++i)
+ constraintsTerminal_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsTerminal_[i]->clone());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical_Raw_Ptr_t
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerAnalytical()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsIntermediate_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as Analytical intermediate constraint " << std::endl;
+ }
+ this->initializedIntermediate_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::addTerminalConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsTerminal_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as Analytical terminal constraint " << std::endl;
+ }
+ this->initializedTerminal_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalIntermediate_.segment(count, constraint_dim) = constraint->evaluate(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+ return evalIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalTerminal_.segment(count, constraint_dim) = constraint->evaluate(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+ return evalTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateConstraintsCount()
+{
+ size_t count = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getTerminalConstraintsCount()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+
+ if (nonZerosState != 0)
+ {
+ evalJacSparseStateIntermediate_.segment(count, nonZerosState) =
+ constraint->jacobianStateSparse(this->x_, this->u_, this->t_);
+ count += nonZerosState;
+ }
+ }
+ return evalJacSparseStateIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateIntermediate()
+{
+ checkIntermediateConstraints();
+
+ MatrixXs jacLocal;
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseStateIntermediate_.block(count, 0, constraint_dim, STATE_DIM) =
+ constraint->jacobianState(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseStateIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseTerminal()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+
+ if (nonZerosState != 0)
+ {
+ evalJacSparseStateTerminal_.segment(count, nonZerosState) =
+ constraint->jacobianStateSparse(this->x_, this->u_, this->t_);
+ count += nonZerosState;
+ }
+ }
+ return evalJacSparseStateTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateTerminal()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseStateTerminal_.block(count, 0, constraint_dim, STATE_DIM) =
+ constraint->jacobianState(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseStateTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+
+ if (nonZerosInput != 0)
+ {
+ evalJacSparseInputIntermediate_.segment(count, nonZerosInput) =
+ constraint->jacobianInputSparse(this->x_, this->u_, this->t_);
+ count += nonZerosInput;
+ }
+ }
+ return evalJacSparseInputIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseInputIntermediate_.block(count, 0, constraint_dim, CONTROL_DIM) =
+ constraint->jacobianInput(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseInputIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+
+ if (nonZerosInput != 0)
+ {
+ evalJacSparseInputTerminal_.segment(count, nonZerosInput) =
+ constraint->jacobianInputSparse(this->x_, this->u_, this->t_);
+ count += nonZerosInput;
+ }
+ }
+ return evalJacSparseInputTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseInputTerminal_.block(count, 0, constraint_dim, CONTROL_DIM) =
+ constraint->jacobianInput(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseInputTerminal_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateIntermediate(
+ Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ checkIntermediateConstraints();
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+ if (nonZerosState > 0)
+ {
+ constraint->sparsityPatternState(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosState);
+ iRowTot.segment(count, nonZerosState) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosState);
+ jColTot.segment(count, nonZerosState) = jColLocal;
+
+ count += nonZerosState;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error(
+ "sparsityPatternStateTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+ if (nonZerosState > 0)
+ {
+ constraint->sparsityPatternState(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosState);
+ iRowTot.segment(count, nonZerosState) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosState);
+ jColTot.segment(count, nonZerosState) = jColLocal;
+
+ count += nonZerosState;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputIntermediate(
+ Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ checkIntermediateConstraints();
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+ if (nonZerosInput > 0)
+ {
+ constraint->sparsityPatternInput(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosInput);
+ iRowTot.segment(count, nonZerosInput) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosInput);
+ jColTot.segment(count, nonZerosInput) = jColLocal;
+
+ count += nonZerosInput;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error(
+ "sparsityPatternInputTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+ if (nonZerosInput > 0)
+ {
+ constraint->sparsityPatternInput(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosInput);
+ iRowTot.segment(count, nonZerosInput) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosInput);
+ jColTot.segment(count, nonZerosInput) = jColLocal;
+
+ count += nonZerosInput;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountIntermediate()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getNumNonZerosJacobianState();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountTerminal()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getNumNonZerosJacobianState();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountIntermediate()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getNumNonZerosJacobianInput();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountTerminal()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getNumNonZerosJacobianInput();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::initializeIntermediate()
+{
+ if (getIntermediateConstraintsCount() > 0)
+ {
+ evalIntermediate_.resize(getIntermediateConstraintsCount());
+ evalIntermediate_.setZero();
+ evalJacSparseStateIntermediate_.resize(getJacobianStateNonZeroCountIntermediate());
+ evalJacSparseStateIntermediate_.setZero();
+ evalJacSparseInputIntermediate_.resize(getJacobianInputNonZeroCountIntermediate());
+ evalJacSparseInputIntermediate_.setZero();
+ evalJacDenseStateIntermediate_.resize(getIntermediateConstraintsCount(), STATE_DIM);
+ evalJacDenseStateIntermediate_.setZero();
+ evalJacDenseInputIntermediate_.resize(getIntermediateConstraintsCount(), CONTROL_DIM);
+ evalJacDenseInputIntermediate_.setZero();
+
+ size_t count = 0;
+
+ this->lowerBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->lowerBoundsIntermediate_.setZero();
+ this->upperBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->upperBoundsIntermediate_.setZero();
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsIntermediate_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsIntermediate_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+ }
+
+ return true;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::initializeTerminal()
+{
+ if (getTerminalConstraintsCount() > 0)
+ {
+ evalTerminal_.resize(getTerminalConstraintsCount());
+ evalTerminal_.setZero();
+ evalJacSparseStateTerminal_.resize(getJacobianStateNonZeroCountTerminal());
+ evalJacSparseStateTerminal_.setZero();
+ evalJacSparseInputTerminal_.resize(getJacobianInputNonZeroCountTerminal());
+ evalJacSparseInputTerminal_.setZero();
+ evalJacDenseStateTerminal_.resize(getTerminalConstraintsCount(), STATE_DIM);
+ evalJacDenseStateTerminal_.setZero();
+ evalJacDenseInputTerminal_.resize(getTerminalConstraintsCount(), CONTROL_DIM);
+ evalJacDenseInputTerminal_.setZero();
+
+ size_t count = 0;
+
+ this->lowerBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->lowerBoundsTerminal_.setZero();
+ this->upperBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->upperBoundsTerminal_.setZero();
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsTerminal_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsTerminal_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::update()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::checkIntermediateConstraints()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Error: Intermediate constraints are or not initialized yet. ");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::checkTerminalConstraints()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Error: Terminal constraints are either not initialized yet. ");
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h
new file mode 100644
index 0000000..ddefee6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h
@@ -0,0 +1,167 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cppad/example/cppad_eigen.hpp>
+
+#include "LinearConstraintContainer.h"
+#include "term/ConstraintBase.h"
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Contains all the constraints using analytically calculated
+ * jacobians
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerAnalytical : public LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintContainerAnalytical_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ ConstraintContainerAnalytical();
+
+ /**
+ * @brief Constructor using state, control and time
+ *
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ ConstraintContainerAnalytical(const state_vector_t& x, const input_vector_t& u, const SCALAR& t = 0.0);
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param arg constraint class to copy
+ */
+ ConstraintContainerAnalytical(const ConstraintContainerAnalytical& arg);
+
+ /**
+ * @brief Deep-cloning of Constraint
+ *
+ * @return Copy of this object.
+ */
+ virtual ConstraintContainerAnalytical_Raw_Ptr_t clone() const override;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintContainerAnalytical();
+
+ /**
+ * @brief Adds an intermedaite constraint.
+ *
+ * @param[in] constraint The constraint to be added
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addIntermediateConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ /**
+ * @brief Adds a terminal constraint.
+ *
+ * @param[in] constraint The constraint to be added
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addTerminalConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ virtual VectorXs evaluateIntermediate() override;
+
+ virtual VectorXs evaluateTerminal() override;
+
+ virtual size_t getIntermediateConstraintsCount() override;
+
+ virtual size_t getTerminalConstraintsCount() override;
+
+ virtual VectorXs jacobianStateSparseIntermediate() override;
+
+ virtual MatrixXs jacobianStateIntermediate() override;
+
+ virtual VectorXs jacobianStateSparseTerminal() override;
+
+ virtual MatrixXs jacobianStateTerminal() override;
+
+ virtual VectorXs jacobianInputSparseIntermediate() override;
+
+ virtual MatrixXs jacobianInputIntermediate() override;
+
+ virtual VectorXs jacobianInputSparseTerminal() override;
+
+ virtual MatrixXs jacobianInputTerminal() override;
+
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual size_t getJacobianStateNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianStateNonZeroCountTerminal() override;
+
+ virtual size_t getJacobianInputNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianInputNonZeroCountTerminal() override;
+
+ virtual bool initializeIntermediate() override;
+
+ virtual bool initializeTerminal() override;
+
+
+private:
+ virtual void update() override;
+
+ /**
+ * @brief Checks whether the intermediate constraints are initialized.
+ * Throws a runtime error if not.
+ */
+ void checkIntermediateConstraints();
+
+ /**
+ * @brief Checks whether the terminal constraints are initialized.
+ * Throws a runtime error if not.
+ */
+ void checkTerminalConstraints();
+
+
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsIntermediate_;
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsTerminal_;
+
+ VectorXs evalIntermediate_;
+ VectorXs evalJacSparseStateIntermediate_;
+ VectorXs evalJacSparseInputIntermediate_;
+ MatrixXs evalJacDenseStateIntermediate_;
+ MatrixXs evalJacDenseInputIntermediate_;
+
+ VectorXs evalTerminal_;
+ VectorXs evalJacSparseStateTerminal_;
+ VectorXs evalJacSparseInputTerminal_;
+ MatrixXs evalJacDenseStateTerminal_;
+ MatrixXs evalJacDenseInputTerminal_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h
new file mode 100644
index 0000000..c58e22a
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h
@@ -0,0 +1,133 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerBase()
+ : x_(state_vector_t::Zero()), u_(input_vector_t::Zero()), t_(SCALAR(0.0))
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerBase(const ConstraintContainerBase& arg)
+ : x_(arg.x_),
+ u_(arg.u_),
+ t_(arg.t_),
+ lowerBoundsIntermediate_(arg.lowerBoundsIntermediate_),
+ lowerBoundsTerminal_(arg.lowerBoundsTerminal_),
+ upperBoundsIntermediate_(arg.upperBoundsIntermediate_),
+ upperBoundsTerminal_(arg.upperBoundsTerminal_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerBase()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR t)
+{
+ t_ = t;
+ x_ = x;
+ u_ = u;
+ update();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintsCount()
+{
+ return getIntermediateConstraintsCount() + getTerminalConstraintsCount();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsIntermediate() const
+{
+ return lowerBoundsIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsTerminal() const
+{
+ return lowerBoundsTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsIntermediate() const
+{
+ return upperBoundsIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsTerminal() const
+{
+ return upperBoundsTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(upperBoundsIntermediate_.rows());
+ return (evaluateIntermediate() - upperBoundsIntermediate_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsIntermediate_.rows());
+ return (evaluateIntermediate() - lowerBoundsIntermediate_).array().min(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(upperBoundsTerminal_.rows());
+ return (evaluateTerminal() - upperBoundsTerminal_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsTerminal_.rows());
+ return (evaluateTerminal() - lowerBoundsTerminal_).array().min(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getTotalBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsIntermediate_.rows());
+ VectorXs eval = evaluateIntermediate();
+ return (eval - lowerBoundsIntermediate_).array().min(vZero.array()) +
+ (eval - upperBoundsIntermediate_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getTotalBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsTerminal_.rows());
+ VectorXs eval = evaluateTerminal();
+ return (eval - lowerBoundsTerminal_).array().min(vZero.array()) +
+ (eval - upperBoundsTerminal_).array().max(vZero.array());
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h
new file mode 100644
index 0000000..293c342
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h
@@ -0,0 +1,210 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/** \defgroup Constraint Constraint
+ *
+ * \brief Constraint Module used in Optimal Control
+ */
+
+/**
+ * @ingroup Constraint
+ * + { list_item_description }
+ *
+ * @brief The ConstraintBase Class is the base class for defining the
+ * non-linear optimization constraints.
+ *
+ * @tparam STATE_DIM Dimension of the state vector
+ * @tparam CONTROL_DIM Dimension of the control input vector
+ *
+ * An example for usage is given in the unit test @ref
+ * ConstraintTest.h
+ */
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintBase_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * \brief Default constructor
+ *
+ * Default constructor, sets state, control and time to zero
+ */
+ ConstraintContainerBase();
+
+ /**
+ * \brief copy constructor
+ *
+ * Copy constructor
+ */
+ ConstraintContainerBase(const ConstraintContainerBase& arg);
+
+ /**
+ * \brief Destructor
+ *
+ * Destructor
+ */
+ virtual ~ConstraintContainerBase();
+
+ /**
+ * Clones the constraint.
+ * @return Base pointer to the clone
+ */
+ virtual ConstraintBase_Raw_Ptr_t clone() const = 0;
+
+ /**
+ * This methods updates the current time, state, and input in the class. It also call the user defined update() method,
+ * which can be used to make the implementation more efficient by executing shared calculations directly at the time of
+ * updating time, state and control.
+ *
+ * @param[in] t current time
+ * @param[in] x state vector
+ * @param[in] x input vector
+ */
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR t = SCALAR(0.0));
+
+ /**
+ * @brief Evaluates the intermediate constraints
+ *
+ * @return The evaluation of the intermediate constraints
+ */
+ virtual VectorXs evaluateIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the terminal constraints
+ *
+ * @return The evaluation of the terminal constraints
+ */
+ virtual VectorXs evaluateTerminal() = 0;
+
+ /**
+ * @brief Retrieves the number of intermediate constraints
+ *
+ * @return The number of intermediate constraints
+ */
+ virtual size_t getIntermediateConstraintsCount() = 0;
+
+ /**
+ * @brief Retrieves the number of final constraints
+ *
+ * @return The number of final constraints
+ */
+ virtual size_t getTerminalConstraintsCount() = 0;
+
+ /**
+ * @brief Retrieves the total number of constraints
+ *
+ * @return The total number of constraints
+ */
+ size_t getConstraintsCount();
+ /**
+ * @brief Retrieves the lower constraint bound on the intermediate
+ * constraints
+ *
+ * @return The lower bound on the intermediate constraints
+ */
+ VectorXs getLowerBoundsIntermediate() const;
+
+ /**
+ * @brief Retrieves the lower constraint bound on the terminal
+ * constraints
+ *
+ * @return The lower bound on the terminal constraints
+ */
+ VectorXs getLowerBoundsTerminal() const;
+
+ /**
+ * @brief Retrieves the upper constraint bound on the intermediate
+ * constraints
+ *
+ * @return The upper bound on the intermediate constraints
+ */
+ VectorXs getUpperBoundsIntermediate() const;
+
+ /**
+ * @brief Retrieves the upper constraint bound on the terminal
+ * constraints
+ *
+ * @return The upper bound on the terminal constraints
+ */
+ VectorXs getUpperBoundsTerminal() const;
+
+ /**
+ * @brief Retrieves the violation of the upper constraint bound on the intermediate constraints
+ *
+ * @return The upper bound violation on intermediate constraints
+ */
+ VectorXs getUpperBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the violation of the lower constraint bound on the intermediate constraints
+ *
+ * @return The lower bound violation on intermediate constraints
+ */
+ VectorXs getLowerBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the violation of the upper constraint bound on the terminal constraints
+ *
+ * @return The upper bound violation on terminal constraints
+ */
+ VectorXs getUpperBoundsViolationTerminal();
+
+ /**
+ * @brief Retrieves the violation of the lower constraint bound on the terminal constraints
+ *
+ * @return The lower bound violation on terminal constraints
+ */
+ VectorXs getLowerBoundsViolationTerminal();
+
+ /**
+ * @brief Retrieves the total violation of the constraints bounds on the intermediate constraints
+ *
+ * @return The total bound violation on intermediate constraints
+ */
+ VectorXs getTotalBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the total violation of the constraints bounds on the terminal constraints
+ *
+ * @return The total bound violation on terminal constraints
+ */
+ VectorXs getTotalBoundsViolationTerminal();
+
+protected:
+ /**
+ * @brief Gets called by the setCurrentStateAndControl method. Can be
+ * used to update container properties
+ */
+ virtual void update() = 0;
+
+ state_vector_t x_; /** state vector */
+ input_vector_t u_; /** control vector */
+ SCALAR t_; /** time */
+
+ VectorXs lowerBoundsIntermediate_;
+ VectorXs lowerBoundsTerminal_;
+ VectorXs upperBoundsIntermediate_;
+ VectorXs upperBoundsTerminal_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h
new file mode 100644
index 0000000..fe2063b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h
@@ -0,0 +1,138 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::LinearConstraintContainer()
+ : initializedIntermediate_(false), initializedTerminal_(false)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::LinearConstraintContainer(
+ const LinearConstraintContainer& arg)
+ : ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ initializedIntermediate_(arg.initializedIntermediate_),
+ initializedTerminal_(arg.initializedTerminal_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::~LinearConstraintContainer()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::getJacNonZeroCount()
+{
+ return getJacobianStateNonZeroCountIntermediate() + getJacobianStateNonZeroCountTerminal() +
+ getJacobianInputNonZeroCountIntermediate() + getJacobianInputNonZeroCountTerminal();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ initializedIntermediate_ = initializeIntermediate();
+ initializedTerminal_ = initializeTerminal();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::isInitialized()
+{
+ return initializedIntermediate_ && initializedTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::printout()
+{
+ Eigen::VectorXi iRows, jCols; // local var
+
+ std::cout << std::endl;
+
+ std::cout << "LinearConstraintContainer - Printout INTERMEDIATE STAGE" << std::endl;
+
+ std::cout << "getLowerBoundsIntermediate().transpose() :" << std::endl;
+ std::cout << this->getLowerBoundsIntermediate().transpose() << std::endl;
+
+ std::cout << "getUpperBoundsIntermediate().transpose() :" << std::endl;
+ std::cout << this->getUpperBoundsIntermediate().transpose() << std::endl;
+
+ std::cout << "getJacobianStateNonZeroCountIntermediate() :" << std::endl;
+ std::cout << getJacobianStateNonZeroCountIntermediate() << std::endl;
+
+ std::cout << "getJacobianInputNonZeroCountIntermediate() :" << std::endl;
+ std::cout << getJacobianInputNonZeroCountIntermediate() << std::endl;
+
+ std::cout << "jacobianStateIntermediate() :" << std::endl;
+ std::cout << jacobianStateIntermediate() << std::endl;
+
+ std::cout << "jacobianStateSparseIntermediate() :" << std::endl;
+ std::cout << jacobianStateSparseIntermediate() << std::endl;
+
+ std::cout << "jacobianInputIntermediate() :" << std::endl;
+ std::cout << jacobianInputIntermediate() << std::endl;
+
+ std::cout << "jacobianInputSparseIntermediate() :" << std::endl;
+ std::cout << jacobianInputSparseIntermediate() << std::endl;
+
+ std::cout << "sparsityPatternStateIntermediate(iRows, jCols) :" << std::endl;
+ sparsityPatternStateIntermediate(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << "sparsityPatternInputIntermediate(iRows, jCols) :" << std::endl;
+ sparsityPatternInputIntermediate(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+
+ std::cout << "LinearConstraintContainer - Printout TERMINAL STAGE" << std::endl;
+
+ std::cout << "getLowerBoundsTerminal().transpose() :" << std::endl;
+ std::cout << this->getLowerBoundsTerminal().transpose() << std::endl;
+
+ std::cout << "getUpperBoundsTerminal().transpose() :" << std::endl;
+ std::cout << this->getUpperBoundsTerminal().transpose() << std::endl;
+
+ std::cout << "getJacobianStateNonZeroCountTerminal() :" << std::endl;
+ std::cout << getJacobianStateNonZeroCountTerminal() << std::endl;
+
+ std::cout << "getJacobianInputNonZeroCountTerminal() :" << std::endl;
+ std::cout << getJacobianInputNonZeroCountTerminal() << std::endl;
+
+ std::cout << "jacobianStateTerminal() :" << std::endl;
+ std::cout << jacobianStateTerminal() << std::endl;
+
+ std::cout << "jacobianStateSparseTerminal() :" << std::endl;
+ std::cout << jacobianStateSparseTerminal() << std::endl;
+
+ std::cout << "jacobianInputTerminal() :" << std::endl;
+ std::cout << jacobianInputTerminal() << std::endl;
+
+ std::cout << "jacobianInputSparseTerminal() :" << std::endl;
+ std::cout << jacobianInputSparseTerminal() << std::endl;
+
+ std::cout << "sparsityPatternStateTerminal(iRows, jCols) :" << std::endl;
+ sparsityPatternStateTerminal(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << "sparsityPatternInputTerminal(iRows, jCols) :" << std::endl;
+ sparsityPatternInputTerminal(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << std::endl;
+ std::cout << std::endl;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h
new file mode 100644
index 0000000..74d560c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h
@@ -0,0 +1,244 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ConstraintContainerBase.h"
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief A base function for linear constraint functions which have a
+ * first derivative
+ *
+ * * The LinearConstraintBase Class is the base class for defining the
+ * non-linear optimization constraints.
+ *
+ * @tparam STATE_DIM Dimension of the state vector
+ * @tparam CONTROL_DIM Dimension of the control input vector
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class LinearConstraintContainer : public ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>* LinearConstraintContainer_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Default constructor
+ */
+ LinearConstraintContainer();
+
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param[in] arg The object to be copied
+ */
+ LinearConstraintContainer(const LinearConstraintContainer& arg);
+ /**
+ * @brief Destructor
+ *
+ */
+ virtual ~LinearConstraintContainer();
+
+ /**
+ * Clones the linear constraint class
+ * @return pointer to the clone
+ */
+ virtual LinearConstraintContainer_Raw_Ptr_t clone() const = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianStateSparseIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state
+ *
+ * @return The jacobian wrt state
+ */
+ virtual MatrixXs jacobianStateIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianStateSparseTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state
+ *
+ * @return The jacobian wrt state
+ */
+ virtual MatrixXs jacobianStateTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianInputSparseIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input
+ *
+ * @return The jacobian wrt control
+ */
+ virtual MatrixXs jacobianInputIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianInputSparseTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input
+ *
+ * @return The jacobian wrt control
+ */
+ virtual MatrixXs jacobianInputTerminal() = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt state
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt state
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt control
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt control
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt state
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianStateNonZeroCountIntermediate() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt state
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianStateNonZeroCountTerminal() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt input
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianInputNonZeroCountIntermediate() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt input
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianInputNonZeroCountTerminal() = 0;
+
+ /**
+ * @brief Returns the number of non zeros in the constraint jacobian
+ * wrt to state and input
+ *
+ * @return The number of the non zeros
+ */
+ size_t getJacNonZeroCount();
+ /**
+ * @brief Initializes the constraint container
+ */
+ void initialize();
+
+ /**
+ * @brief Initializes the intermediate constraints
+ *
+ * @return Returns true if the initialization was successful
+ */
+ virtual bool initializeIntermediate() = 0;
+
+ /**
+ * @brief Initializes the terminal constraints
+ *
+ * @return Returns true if the initialization was successful
+ */
+ virtual bool initializeTerminal() = 0;
+
+ /**
+ * @brief Checks if the constraint container is initialized
+ *
+ * @return Returns true if initialized
+ */
+ bool isInitialized();
+
+ /**
+ * @brief Print out sparsity patterns, jacobians, etc. Serves for quick visual inspection
+ */
+ void printout();
+
+protected:
+ bool initializedIntermediate_;
+ bool initializedTerminal_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/constraint-impl.h b/ct_optcon/include/ct/optcon/constraint/constraint-impl.h
new file mode 100644
index 0000000..6cd775d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/constraint-impl.h
@@ -0,0 +1,18 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "term/ConstraintBase-impl.h"
+#include "term/ControlInputConstraint-impl.h"
+#include "term/ObstacleConstraint-impl.h"
+#include "term/StateConstraint-impl.h"
+#include "term/TerminalConstraint-impl.h"
+
+#include "ConstraintContainerBase-impl.h"
+#include "LinearConstraintContainer-impl.h"
+#include "ConstraintContainerAnalytical-impl.h"
+#include "ConstraintContainerAD-impl.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/constraint.h b/ct_optcon/include/ct/optcon/constraint/constraint.h
new file mode 100644
index 0000000..f354d14
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/constraint.h
@@ -0,0 +1,19 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "term/ConstraintBase.h"
+#include "term/BoxConstraintBase.h"
+#include "term/ControlInputConstraint.h"
+#include "term/ObstacleConstraint.h"
+#include "term/StateConstraint.h"
+#include "term/TerminalConstraint.h"
+
+#include "ConstraintContainerBase.h"
+#include "LinearConstraintContainer.h"
+#include "ConstraintContainerAD.h"
+#include "ConstraintContainerAnalytical.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
new file mode 100644
index 0000000..c143490
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
@@ -0,0 +1,135 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const decision_vector_t& lb,
+ const decision_vector_t& ub)
+{
+ // check if box constraints are meaningful
+ sanityCheck(DERIVED_DIM, lb, ub);
+
+ // this box constraint is 'dense' (one dense jacobian diagonal, one zero jacobian)
+ constrSize_ = DERIVED_DIM;
+ Base::lb_.resize(DERIVED_DIM);
+ Base::ub_.resize(DERIVED_DIM);
+ sparsity_ = Eigen::Matrix<int, DERIVED_DIM, 1>::Ones();
+ sparsity_J_.resize(DERIVED_DIM, DERIVED_DIM);
+ sparsity_J_.setIdentity();
+ Base::lb_ = lb.template cast<SCALAR>();
+ Base::ub_ = ub.template cast<SCALAR>();
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& sparsity_vec)
+{
+ // make sure the provided sparsity pattern is correct and consists only of ones and zeros
+ assert(sparsity_vec.maxCoeff() <= 1);
+ assert(sparsity_vec.minCoeff() >= 0);
+
+ constrSize_ = (size_t)sparsity_vec.sum();
+
+ // make sure the provided bounds are consistent
+ sanityCheck(constrSize_, lb, ub);
+
+ Base::lb_.resize(constrSize_);
+ Base::ub_.resize(constrSize_);
+ sparsity_ = sparsity_vec;
+ sparsity_J_.resize(constrSize_, DERIVED_DIM);
+ sparsity_J_ = diagSparsityVecToSparsityMat(sparsity_vec, constrSize_);
+ Base::lb_ = lb.template cast<SCALAR>();
+ Base::ub_ = ub.template cast<SCALAR>();
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const BoxConstraintBase& arg)
+ : Base(arg), sparsity_(arg.sparsity_), sparsity_J_(arg.sparsity_J_), constrSize_(arg.constrSize_)
+{
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::~BoxConstraintBase()
+{
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return constrSize_;
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sparsity_matrix_t
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::diagSparsityVecToSparsityMat(const VectorXi& spVec,
+ const size_t& nConstr)
+{
+ // set up sparsity matrix all zero
+ sparsity_matrix_t mat(nConstr, DERIVED_DIM);
+ mat.setZero();
+
+ // set selected elements to one
+ size_t count = 0;
+ assert(spVec.rows() == DERIVED_DIM);
+ for (size_t i = 0; i < DERIVED_DIM; i++)
+ {
+ if (spVec(i) == 1)
+ {
+ mat(count, i) = 1;
+ count++;
+ }
+ }
+ return mat;
+}
+
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sanityCheck(const size_t& nCon,
+ const VectorXs& lb,
+ const VectorXs& ub) const
+{
+ // assert that the size of constraint vectors is equal to the computed/given number of constraints
+ if (lb.rows() != nCon | ub.rows() != nCon)
+ {
+ std::cout << "no. Constraints: " << nCon << std::endl;
+ std::cout << "BoxConstraintBase: lb " << lb.transpose() << std::endl;
+ std::cout << "BoxConstraintBase: ub " << ub.transpose() << std::endl;
+ throw std::runtime_error("BoxConstraintBase: wrong constraint sizes in StateConstraint");
+ }
+
+ // assert that the boundaries are meaningful
+ for (size_t i = 0; i < nCon; i++)
+ {
+ if (lb(i) > ub(i))
+ {
+ std::cout << "BoxConstraintBase: lb " << lb.transpose() << std::endl;
+ std::cout << "BoxConstraintBase: ub " << ub.transpose() << std::endl;
+ throw std::runtime_error("BoxConstraintBase: wrong boundaries: lb > ub");
+ }
+ }
+}
+
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternSparseJacobian(
+ const VectorXi& sparsity_vec,
+ const size_t& constrSize,
+ VectorXi& rows,
+ VectorXi& cols)
+{
+ Base::genSparseDiagonalIndices(sparsity_vec, rows, cols);
+ for (size_t i = 0; i < constrSize; i++)
+ rows(i) = i;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h
new file mode 100644
index 0000000..ea38774
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @brief Base for box constraint, templated on dimension of the decision vector of the derived class
+ *
+ * @tparam DERIVED_DIM Dimension of the decision vector of the derived class
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class BoxConstraintBase : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using decision_vector_t = core::StateVector<DERIVED_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, DERIVED_DIM>;
+
+ //! generate sparsity pattern for sparse box constraint
+ static void sparsityPatternSparseJacobian(const VectorXi& sparsity_vec,
+ const size_t& constrSize,
+ VectorXi& rows,
+ VectorXi& cols);
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes the box constraint is dense.
+ *
+ * @param[in] vLow The full lower bound
+ * @param[in] vHigh The full upper bound
+ */
+ BoxConstraintBase(const decision_vector_t& vLow, const decision_vector_t& vHigh);
+
+ /**
+ * @brief Constructor for sparse box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param sparsity_vec Box constraint sparsity pattern as a vector
+ */
+ BoxConstraintBase(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& sparsity_vec);
+
+ BoxConstraintBase(const BoxConstraintBase& arg);
+
+ virtual ~BoxConstraintBase();
+
+ virtual BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override = 0;
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override = 0;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual size_t getNumNonZerosJacobianState() const override = 0;
+
+ virtual size_t getNumNonZerosJacobianInput() const override = 0;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override = 0;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override = 0;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override = 0;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override = 0;
+
+protected:
+ /*!
+ * @brief transform a sparsity vector (giving the sparsity pattern on the diagonal) in to a sparsity matrix
+ * @param spVec diagonal sparsity pattern, e.g. [0 0 1 0 1 0]
+ * @param nConstr number of constraints
+ * @return the sparsity matrix
+ */
+ sparsity_matrix_t diagSparsityVecToSparsityMat(const VectorXi& spVec, const size_t& nConstr);
+
+ //! sparsity in vector form
+ VectorXi sparsity_;
+
+ //! sparsity matrix
+ sparsity_matrix_t sparsity_J_;
+
+ //! size of the constraint
+ size_t constrSize_;
+
+private:
+ void sanityCheck(const size_t& nCon, const VectorXs& lb, const VectorXs& ub) const;
+};
+
+} // namespace optcon
+} // namepace ct
+
+/*
+ * For this class, we can include the implementation here, as it is virtual,
+ * and only used by few derived members who all get compiled in prespec themselves.
+ */
+#include "BoxConstraintBase-impl.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h
new file mode 100644
index 0000000..4226bbd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h
@@ -0,0 +1,206 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintBase(std::string name) : name_(name)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintBase(const ConstraintBase& arg)
+ : lb_(arg.lb_), ub_(arg.ub_), name_(arg.name_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintBase()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ throw std::runtime_error("Term " + name_ + " has no Implementation of evaluateCppaCg.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ throw std::runtime_error(
+ "This constraint function element is not implemented for the given term."
+ "Please use either auto-diff cost function or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ throw std::runtime_error(
+ "This constraint function element is not implemented for the given term."
+ "Please use either auto-diff cost function or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBound() const
+{
+ return lb_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBound() const
+{
+ return ub_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getName(std::string& constraintName) const
+{
+ constraintName = name_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::setName(const std::string constraintName)
+{
+ name_ = constraintName;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return STATE_DIM * getConstraintSize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return CONTROL_DIM * getConstraintSize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jacState = jacobianState(x, u, t);
+
+ VectorXs jac(Eigen::Map<VectorXs>(jacState.data(), jacState.rows() * jacState.cols()));
+
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jacInput = jacobianInput(x, u, t);
+
+ VectorXs jac(Eigen::Map<VectorXs>(jacInput.data(), jacInput.rows() * jacInput.cols()));
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(Eigen::VectorXi& rows, Eigen::VectorXi& cols)
+{
+ genBlockIndices(getConstraintSize(), STATE_DIM, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(Eigen::VectorXi& rows, Eigen::VectorXi& cols)
+{
+ genBlockIndices(getConstraintSize(), CONTROL_DIM, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genDiagonalIndices(const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ iRow_vec.resize(num_elements);
+ jCol_vec.resize(num_elements);
+
+ size_t count = 0;
+
+ for (size_t i = 0; i < num_elements; ++i)
+ {
+ iRow_vec(count) = i;
+ jCol_vec(count) = i;
+ count++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genSparseDiagonalIndices(const Eigen::VectorXi& diag_sparsity,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ // make sure the sparsity pattern is correct and consists only of ones and zeros
+ assert(diag_sparsity.maxCoeff() <= 1);
+ assert(diag_sparsity.minCoeff() >= 0);
+
+ const int num_elements = diag_sparsity.sum();
+
+ iRow_vec.resize(num_elements);
+ jCol_vec.resize(num_elements);
+
+ size_t count = 0;
+
+ for (int i = 0; i < diag_sparsity.rows(); ++i)
+ {
+ if (diag_sparsity(i) == 1)
+ {
+ iRow_vec(count) = i;
+ jCol_vec(count) = i;
+ count++;
+ }
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genBlockIndices(const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ size_t num_gen_indices = num_rows * num_cols;
+
+ iRow_vec.resize(num_gen_indices);
+ jCol_vec.resize(num_gen_indices);
+
+ size_t count = 0;
+
+ for (size_t row = 0; row < num_rows; ++row)
+ {
+ for (size_t col = 0; col < num_cols; ++col)
+ {
+ iRow_vec(count) = row;
+ jCol_vec(count) = col;
+ count++;
+ }
+ }
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h
new file mode 100644
index 0000000..6dff79b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h
@@ -0,0 +1,243 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Base class for the constraints used in this toolbox
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] name The name of the constraint
+ */
+ ConstraintBase(std::string name = "Unnamed");
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param[in] arg The object to be copied
+ */
+ ConstraintBase(const ConstraintBase& arg);
+
+ /**
+ * @brief Creates a new instance of the object with same properties than original.
+ *
+ * @return Copy of this object.
+ */
+ virtual ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintBase();
+
+ /**
+ * @brief The evaluation of the constraint violation. Note this method
+ * is SCALAR typed
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The constraint violation
+ */
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) = 0;
+
+ /**
+ * @brief The evaluate method used for jit compilation in constraint
+ * container ad
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The constraint violation
+ */
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t);
+
+
+ /**
+ * @brief Returns the number of constraints
+ *
+ * @return The number of constraints
+ */
+ virtual size_t getConstraintSize() const = 0;
+
+
+ /**
+ * @brief Returns the constraint jacobian wrt state
+ *
+ * @return The constraint jacobian
+ */
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the constraint jacobian wrt input
+ *
+ * @return The constraint jacobian
+ */
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the lower constraint bound
+ *
+ * @return The lower constraint bound
+ */
+ virtual VectorXs getLowerBound() const;
+
+ /**
+ * @brief Returns the upper constraint bound
+ *
+ * @return The upper constraint bound
+ */
+ virtual VectorXs getUpperBound() const;
+
+ /**
+ * @brief Returns the constraint name
+ *
+ * @param[out] constraintName The constraint name
+ */
+ void getName(std::string& constraintName) const;
+
+ /**
+ * @brief Sets the constraint name.
+ *
+ * @param[in] constraintName The constraint name
+ */
+ void setName(const std::string constraintName);
+
+ /**
+ * @brief Returns the number of nonzeros in the jacobian wrt state. The
+ * default implementation assumes a dense matrix with only
+ * nonzero elements.
+ *
+ * @return The number of non zeros
+ */
+ virtual size_t getNumNonZerosJacobianState() const;
+
+ /**
+ * @brief Returns the number of nonzeros in the jacobian wrt control
+ * input. The default implementation assumes a dense matrix with
+ * only nonzero elements
+ *
+ * @return The number of non zeros
+ */
+ virtual size_t getNumNonZerosJacobianInput() const;
+
+ /**
+ * @brief Returns the constraint jacobian wrt state in sparse
+ * structure. The default implementation maps the JacobianState
+ * matrix to a vector
+ *
+ * @return The sparse constraint jacobian
+ */
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the constraint jacobian wrt control input in sparse
+ * structure. The default implementation maps the JacobianState
+ * matrix to a vector
+ *
+ * @return The sparse constraint jacobian
+ */
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+
+ /**
+ * @brief Generates the sparsity pattern of the jacobian wrt state. The
+ * default implementation returns a vector of ones corresponding
+ * to the dense jacobianState
+ *
+ * @param rows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param cols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ */
+ virtual void sparsityPatternState(Eigen::VectorXi& rows, Eigen::VectorXi& cols);
+
+ /**
+ * @brief Generates the sparsity pattern of the jacobian wrt control
+ * input. The default implementation returns a vector of ones
+ * corresponding to the dense jacobianInput
+ *
+ * @param rows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param cols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ */
+ virtual void sparsityPatternInput(Eigen::VectorXi& rows, Eigen::VectorXi& cols);
+
+
+protected:
+ VectorXs lb_; //! lower bound on the constraints
+ VectorXs ub_; //! upper bound on the constraints
+
+ /**
+ * @brief Generates indices of a diagonal square matrix
+ *
+ * @param[in] num_elements The number of elements
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The column vector
+ */
+ static void genDiagonalIndices(const size_t num_elements, Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec);
+
+ /**
+ * @brief Generates indices of a sparse diagonal square matrix
+ *
+ * @param[in] diag_sparsity Sparsity pattern for the diagonal (Example: [0 1 0 0 1 1])
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The column vector
+ */
+ static void genSparseDiagonalIndices(const Eigen::VectorXi& diag_sparsity,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec);
+
+ /**
+ * @brief Generates indices of a full matrix
+ *
+ * @param[in] num_rows The number of rows of the matrix
+ * @param[in] num_cols The number columns of the matrix
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The col vector
+ */
+ static void genBlockIndices(const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec);
+
+private:
+ std::string name_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h
new file mode 100644
index 0000000..4fb8dd1
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h
@@ -0,0 +1,130 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const control_vector_t& lb,
+ const control_vector_t& ub)
+ : Base(lb, ub)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& control_sparsity)
+ : Base(lb, ub, control_sparsity)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const ControlInputConstraint& arg)
+ : Base(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~ControlInputConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone()
+ const
+{
+ return new ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(
+ const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_ * u;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return this->sparsity_J_.template cast<ct::core::ADCGScalar>() * u;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jac(this->constrSize_, STATE_DIM);
+ jac.setZero();
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return this->constrSize_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return VectorXs();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ // do nothing
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ VectorXs jac(this->constrSize_);
+ jac.setConstant(1.0);
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(VectorXi& rows, VectorXi& cols)
+{
+ this->sparsityPatternSparseJacobian(this->sparsity_, this->constrSize_, rows, cols);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h
new file mode 100644
index 0000000..07cda6d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h
@@ -0,0 +1,86 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for control input box constraint term
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ControlInputConstraint : public BoxConstraintBase<CONTROL_DIM, STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = BoxConstraintBase<CONTROL_DIM, STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, CONTROL_DIM>;
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes state box constraint is dense.
+ *
+ * @param[in] uLow The upper control input bound
+ * @param[in] uHigh The lower control input bound
+ */
+ ControlInputConstraint(const control_vector_t& uLow, const control_vector_t& uHigh);
+
+ /**
+ * @brief Constructor for sparse control input box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param control_sparsity Control input constraint sparsity pattern
+ */
+ ControlInputConstraint(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& control_sparsity);
+
+ ControlInputConstraint(const ControlInputConstraint& arg);
+
+ virtual ~ControlInputConstraint();
+
+ virtual ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const override;
+
+ virtual size_t getNumNonZerosJacobianInput() const override;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h
new file mode 100644
index 0000000..b07247e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h
@@ -0,0 +1,91 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
+#define CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ObstacleConstraint(
+ std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle,
+ std::function<void(const state_vector_t&, Vector3s&)> getPosition,
+ std::function<void(const state_vector_t&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> getJacobian)
+ : obstacle_(obstacle), xFun_(getPosition), dXFun_(getJacobian)
+{
+ this->lb_.resize(1);
+ this->ub_.resize(1);
+ this->lb_(0) = SCALAR(0.0);
+ this->ub_(0) = std::numeric_limits<SCALAR>::max();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~ObstacleConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ObstacleConstraint(const ObstacleConstraint& arg)
+ : Base(arg), obstacle_(arg.obstacle_), xFun_(arg.xFun_), dXFun_(arg.dXFun_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return 1;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(
+ const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ Vector3s xRef;
+ xFun_(x, xRef);
+ val_(0) = obstacle_->insideEllipsoid(xRef);
+ return val_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ Vector3s xRef;
+ Eigen::Matrix<SCALAR, 3, STATE_DIM> dXRef;
+ xFun_(x, xRef);
+ dXFun_(x, dXRef);
+ VectorXs dist = xRef - obstacle_->x0();
+ jac_ = 2 * dist.transpose() * obstacle_->S() * obstacle_->A().transpose() * obstacle_->A() *
+ obstacle_->S().transpose() * dXRef;
+ return jac_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, 1, CONTROL_DIM>::Zero();
+}
+
+
+} // namespace optcon
+} // namespace core
+
+#endif //CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h
new file mode 100644
index 0000000..ce4773f
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h
@@ -0,0 +1,67 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @brief Class for obstacle constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ObstacleConstraint : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
+
+ ObstacleConstraint(std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle,
+ std::function<void(const state_vector_t&, Vector3s&)> getPosition,
+ std::function<void(const state_vector_t&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> getJacobian);
+
+ virtual ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual ~ObstacleConstraint();
+
+ ObstacleConstraint(const ObstacleConstraint& arg);
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+private:
+ std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle_;
+
+ std::function<void(const core::StateVector<STATE_DIM, SCALAR>&, Vector3s&)> xFun_;
+ std::function<void(const core::StateVector<STATE_DIM, SCALAR>&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> dXFun_;
+
+ core::StateVector<1, SCALAR> val_;
+ Eigen::Matrix<SCALAR, 1, STATE_DIM> jac_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h
new file mode 100644
index 0000000..6f9a6e4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h
@@ -0,0 +1,126 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const state_vector_t& xLow,
+ const state_vector_t& xHigh)
+ : Base(xLow, xHigh)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& state_sparsity)
+ : Base(lb, ub, state_sparsity)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const StateConstraint& arg) : Base(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~StateConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_ * x;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return this->sparsity_J_.template cast<ct::core::ADCGScalar>() * x;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jac(this->constrSize_, CONTROL_DIM);
+ jac.setZero();
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return this->constrSize_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ VectorXs jac(this->constrSize_);
+ jac.setConstant(1.0);
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ this->sparsityPatternSparseJacobian(this->sparsity_, this->constrSize_, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return VectorXs();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(VectorXi& rows, VectorXi& cols)
+{
+ //do nothing
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h
new file mode 100644
index 0000000..fff7aff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h
@@ -0,0 +1,87 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for state box constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class StateConstraint : public BoxConstraintBase<STATE_DIM, STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = BoxConstraintBase<STATE_DIM, STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, STATE_DIM>;
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes state box constraint is dense.
+ *
+ * @param[in] xLow The lower state bound
+ * @param[in] xHigh The upper state bound
+ */
+ StateConstraint(const state_vector_t& xLow, const state_vector_t& xHigh);
+
+ /**
+ * @brief Constructor for sparse state box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param state_sparsity State constraint sparsity pattern
+ */
+ StateConstraint(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& state_sparsity);
+
+ StateConstraint(const StateConstraint& arg);
+
+ virtual ~StateConstraint();
+
+ virtual StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const override;
+
+ virtual size_t getNumNonZerosJacobianInput() const override;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h
new file mode 100644
index 0000000..2f79335
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h
@@ -0,0 +1,110 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::TerminalConstraint(const core::StateVector<STATE_DIM, SCALAR> xf)
+{
+ xF_ = xf;
+ Base::lb_.resize(STATE_DIM);
+ Base::ub_.resize(STATE_DIM);
+ // The terminal state constraint is treated as equality constraint, therefore, ub = lb
+ Base::lb_.setConstant(SCALAR(0.0));
+ Base::ub_.setConstant(SCALAR(0.0));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::TerminalConstraint(const TerminalConstraint& arg)
+ : Base(arg), xF_(arg.xF_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~TerminalConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return STATE_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return x - xF_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return x - xF_.template cast<ct::core::ADCGScalar>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>::Identity();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM>::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return STATE_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return core::StateVector<STATE_DIM, SCALAR>::Ones();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ this->genDiagonalIndices(STATE_DIM, rows, cols);
+}
+}
+}
diff --git a/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h
new file mode 100644
index 0000000..4343ed7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h
@@ -0,0 +1,74 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for terminal constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class TerminalConstraint : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] xf The desired terminal state
+ */
+ TerminalConstraint(const core::StateVector<STATE_DIM, SCALAR> xf);
+
+ virtual TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ TerminalConstraint(const TerminalConstraint& arg);
+
+ virtual ~TerminalConstraint();
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const;
+
+ virtual size_t getNumNonZerosJacobianInput() const;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+private:
+ core::StateVector<STATE_DIM, SCALAR> xF_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
new file mode 100644
index 0000000..e56067e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
@@ -0,0 +1,55 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunction() : t_(0.0), t_shift_(0.0)
+{
+ x_.setZero();
+ u_.setZero();
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunction(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunction(const CostFunction& arg)
+ : x_(arg.x_), u_(arg.u_), t_(arg.t_), t_shift_(arg.t_shift_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ x_ = x;
+ u_ = u;
+ t_ = t + t_shift_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::getCurrentStateAndControl(Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ SCALAR& t) const
+{
+ x = this->x_;
+ u = this->u_;
+ t = this->t_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::shiftTime(const SCALAR t)
+{
+ t_shift_ = t;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp
new file mode 100644
index 0000000..99b9f08
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp
@@ -0,0 +1,112 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <vector>
+
+namespace ct {
+namespace optcon {
+
+/** \defgroup CostFunction CostFunction
+ *
+ * \brief Cost Function Module used in Optimal Control
+ */
+
+/**
+ * \ingroup CostFunction
+ *+
+ * \brief A base function for cost functions. All cost functions should derive from this.
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunction
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * \brief Default constructor
+ *
+ * Default constructor, sets state, control and time to zero
+ */
+ CostFunction();
+
+ /**
+ * \brief Destructor
+ *
+ * Destructor
+ */
+ virtual ~CostFunction();
+
+ /**
+ * \brief Copy constructor
+ * @param arg other cost function
+ */
+ CostFunction(const CostFunction& arg);
+
+ /**
+ * Clones the cost function.
+ * @return Base pointer to the clone
+ */
+ virtual CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * Set the current state, control and time of the cost function. In this function, the user can add pre-computations
+ * shared between different calls to the derivative functions.
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t = SCALAR(0.0));
+
+ /**
+ * \brief sets current state, control and time
+ *
+ * Get the current state, control and time of the cost function. In this function, the user can add pre-computations
+ * shared between different calls to the derivative functions.
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ virtual void getCurrentStateAndControl(Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ SCALAR& t) const;
+
+ /**
+ * \brief evaluate intermediate costs
+ *
+ * Evaluates the running/intermediate cost function for the control, state and time set in setCurrentStateAndControl()
+ * @return costs
+ */
+ virtual SCALAR evaluateIntermediate() = 0;
+
+ /**
+ * \brief evaluate terminal costs
+ *
+ * Evaluates the terminal cost for a given state and control set in setCurrentStateAndControl(). This usually ignores time.
+ * @return costs
+ */
+ virtual SCALAR evaluateTerminal() = 0;
+
+ virtual void shiftTime(const SCALAR t);
+
+
+protected:
+ state_vector_t x_; /** state vector */
+ control_vector_t u_; /** control vector */
+ SCALAR t_; /** time */
+
+ SCALAR t_shift_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
new file mode 100644
index 0000000..810985b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
@@ -0,0 +1,362 @@
+/**********************************************************************************************************************
+ This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+ Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+ Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD()
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(),
+ stateControlTime_(Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM + 1, 1>::Zero())
+{
+ intermediateFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateIntermediateCg(stateInputTime);
+ };
+
+ finalFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateTerminalCg(stateInputTime);
+ };
+
+ intermediateCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+ finalCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+
+ setCurrentStateAndControl(this->x_, this->u_, this->t_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD(const CostFunctionAD& arg)
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ stateControlTime_(arg.stateControlTime_),
+ intermediateFun_(arg.intermediateFun_),
+ finalFun_(arg.finalFun_)
+{
+ intermediateTerms_.resize(arg.intermediateTerms_.size());
+ finalTerms_.resize(arg.finalTerms_.size());
+
+ for (size_t i = 0; i < intermediateTerms_.size(); ++i)
+ intermediateTerms_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>(arg.intermediateTerms_[i]->clone());
+
+ for (size_t i = 0; i < finalTerms_.size(); ++i)
+ finalTerms_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>(arg.finalTerms_[i]->clone());
+
+ intermediateCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+ finalCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+
+ initialize(); //when cloning, we directly call initialize()
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD(const std::string& filename, bool verbose)
+ : CostFunctionAD() //! @warning the delegating constructor in the initializer list is required to call the initial routine in CostFunctionAD()
+{
+ loadFromConfigFile(filename, verbose);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionAD(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>* CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new CostFunctionAD(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ intermediateFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateIntermediateCg(stateInputTime);
+ };
+
+ finalFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateTerminalCg(stateInputTime);
+ };
+
+ intermediateCostCodegen_->update(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1);
+ finalCostCodegen_->update(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1);
+
+ //! @ todo: this should probably become an option (eg. IPOPT can work without cost Hessians)
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createHessian_ = true;
+
+ finalCostCodegen_->compileJIT(settings, "finalCosts");
+ intermediateCostCodegen_->compileJIT(settings, "intermediateCosts");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose)
+{
+ intermediateTerms_.push_back(term);
+
+ if (verbose)
+ {
+ std::cout << term->getName() + " added as intermediate AD term" << std::endl;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose)
+{
+ finalTerms_.push_back(term);
+
+ if (verbose)
+ {
+ std::cout << term->getName() + " added as final AD term" << std::endl;
+ }
+}
+
+// set state and control
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ this->x_ = x;
+ this->u_ = u;
+ this->t_ = t;
+
+ stateControlTime_ << x, u, t;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename, bool verbose)
+{
+ this->intermediateCostAnalytical_.clear();
+ this->finalCostAnalytical_.clear();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+ int i = 0;
+ std::string currentTerm;
+ do
+ {
+ currentTerm = "term" + std::to_string(i);
+ std::string termKind = pt.get<std::string>(currentTerm + ".kind");
+ boost::algorithm::to_lower(termKind);
+ int currentTermType = pt.get<int>(currentTerm + ".type");
+ std::string termName;
+ try
+ {
+ termName = pt.get<std::string>(currentTerm + ".name");
+ } catch (boost::property_tree::ptree_bad_path err)
+ {
+ termName = "Unnamed";
+ if (verbose)
+ {
+ std::cout << "Name field for " + currentTerm + " does not exist" << std::endl;
+ }
+ }
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term;
+
+ CT_LOADABLE_TERMS(SCALAR, CGScalar);
+
+ if (!term)
+ {
+ throw std::runtime_error("Term type \"" + termKind + "\" not supported");
+ }
+ else
+ {
+ if (term)
+ addADTerm(filename, currentTerm, currentTermType, term, this, verbose);
+ else
+ throw std::runtime_error("Term type \"" + termKind + "\" loaded but unsupported.");
+ }
+ currentTerm = "term" + std::to_string(++i);
+ } while (pt.find(currentTerm) != pt.not_found());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixCg
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateCg(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime)
+{
+ CGScalar y = CGScalar(0.0);
+
+ for (auto it : intermediateTerms_)
+ y += it->evaluateCppadCg(stateInputTime.segment(0, STATE_DIM), stateInputTime.segment(STATE_DIM, CONTROL_DIM),
+ stateInputTime(STATE_DIM + CONTROL_DIM));
+
+ Eigen::Matrix<CGScalar, 1, 1> out;
+ out << y;
+ return out;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixCg
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalCg(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime)
+{
+ CGScalar y = CGScalar(0.0);
+
+ for (auto it : finalTerms_)
+ y += it->evaluateCppadCg(stateInputTime.segment(0, STATE_DIM), stateInputTime.segment(STATE_DIM, CONTROL_DIM),
+ stateInputTime(STATE_DIM + CONTROL_DIM));
+
+ Eigen::Matrix<CGScalar, 1, 1> out;
+ out << y;
+ return out;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ return this->evaluateIntermediateBase() + intermediateCostCodegen_->forwardZero(stateControlTime_)(0);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ return this->evaluateTerminalBase() + finalCostCodegen_->forwardZero(stateControlTime_)(0);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
+ intermediateCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
+ intermediateCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM).transpose() + this->controlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM).transpose() + this->controlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<STATE_DIM, STATE_DIM>(0, 0) + this->stateSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<STATE_DIM, STATE_DIM>(0, 0) + this->stateSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, CONTROL_DIM>(STATE_DIM, STATE_DIM) +
+ this->controlSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, CONTROL_DIM>(STATE_DIM, STATE_DIM) +
+ this->controlSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, STATE_DIM>(STATE_DIM, 0) + this->stateControlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, STATE_DIM>(STATE_DIM, 0) + this->stateControlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<ct::optcon::
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateADTermById(const size_t id)
+{
+ return intermediateTerms_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<ct::optcon::
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalADTermById(const size_t id)
+{
+ return finalTerms_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateADTermByName(const std::string& name)
+{
+ for (auto term : intermediateTerms_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionAD");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalADTermByName(const std::string& name)
+{
+ for (auto term : finalTerms_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionAD");
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
new file mode 100644
index 0000000..833e03c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
@@ -0,0 +1,169 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/core/core.h>
+#include <memory>
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include "CostFunctionQuadratic.hpp"
+#include "utility/utilities.hpp"
+
+#include "term/TermLoadMacros.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief Cost Function with Auto-Diff support
+ *
+ * This cost function can work with both, analytical terms as well as
+ * auto-diff terms. For analytical terms it will use provided derivatives
+ * and for auto-diff terms derivatives will be computed using auto-diff.
+ *
+ * Unit test \ref ADTest.cpp illustrates the use of a CostFunctionAD.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionAD : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::DerivativesCppadJIT<STATE_DIM + CONTROL_DIM + 1, 1> JacCG;
+ typedef typename JacCG::CG_SCALAR CGScalar;
+ typedef Eigen::Matrix<CGScalar, 1, 1> MatrixCg;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * \brief Basic constructor
+ */
+ CostFunctionAD();
+
+ /**
+ * \brief Constructor loading function from file
+ * @param filename config file location
+ * @param verbose flag enabling printouts
+ */
+ CostFunctionAD(const std::string& filename, bool verbose = false);
+
+ /**
+ * Deep-cloning of cost function
+ * @return base pointer to clone
+ */
+ CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const;
+
+ /**
+ * \brief Copy constructor
+ * @param arg cost function to copy
+ */
+ CostFunctionAD(const CostFunctionAD& arg);
+
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~CostFunctionAD();
+
+
+ /**
+ * @brief Initializes the AD costfunction, generates and compiles
+ * source code
+ */
+ virtual void initialize() override;
+
+ /**
+ * \brief Add an intermediate, auto-differentiable term
+ *
+ * Use this function to add an auto-differentiable, intermediate term to the cost function.
+ *
+ * @param term The term to be added
+ * @param verbose Flag enabling printouts
+ * @return
+ */
+ void addIntermediateADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose = false) override;
+
+ /**
+ * \brief Add a final, auto-differentiable term
+ *
+ * Use this function to add an auto-differentiable, final term to the cost function.
+ *
+ * @param term The term to be added
+ * @param verbose Flag enabling printouts
+ * @return
+ */
+ void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose = false) override;
+
+ void setCurrentStateAndControl(const state_vector_t& x, const control_vector_t& u, const SCALAR& t = 0.0) override;
+
+ void loadFromConfigFile(const std::string& filename, bool verbose = false) override;
+
+ SCALAR evaluateIntermediate() override;
+ SCALAR evaluateTerminal() override;
+
+ state_vector_t stateDerivativeIntermediate() override;
+ state_vector_t stateDerivativeTerminal() override;
+
+ control_vector_t controlDerivativeIntermediate() override;
+ control_vector_t controlDerivativeTerminal() override;
+
+ state_matrix_t stateSecondDerivativeIntermediate() override;
+ state_matrix_t stateSecondDerivativeTerminal() override;
+
+ control_matrix_t controlSecondDerivativeIntermediate() override;
+ control_matrix_t controlSecondDerivativeTerminal() override;
+
+ control_state_matrix_t stateControlDerivativeIntermediate() override;
+ control_state_matrix_t stateControlDerivativeTerminal() override;
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermByName(
+ const std::string& name);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermByName(const std::string& name);
+
+
+private:
+ MatrixCg evaluateIntermediateCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);
+ MatrixCg evaluateTerminalCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);
+
+ //! combined state, control and time vector
+ Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM + 1, 1> stateControlTime_;
+
+ //! intermediate AD terms
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> intermediateTerms_;
+ //! final AD terms
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> finalTerms_;
+
+ //! generated jacobians
+ std::shared_ptr<JacCG> intermediateCostCodegen_;
+ std::shared_ptr<JacCG> finalCostCodegen_;
+
+ //! cppad functions
+ typename JacCG::FUN_TYPE_CG intermediateFun_;
+ typename JacCG::FUN_TYPE_CG finalFun_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
new file mode 100644
index 0000000..e0d8a22
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
@@ -0,0 +1,174 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(const CostFunctionAnalytical& arg)
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(const std::string& filename,
+ bool verbose)
+{
+ loadFromConfigFile(filename, verbose);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::clone()
+ const
+{
+ return new CostFunctionAnalytical(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionAnalytical(){};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename,
+ bool verbose)
+{
+ if (verbose)
+ std::cout << "Starting to load analytical cost function from file " << filename << std::endl;
+
+ this->intermediateCostAnalytical_.clear();
+ this->finalCostAnalytical_.clear();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+ int i = 0;
+ std::string currentTerm;
+ do
+ {
+ std::cout << "=============================================" << std::endl; //indicating new term
+ currentTerm = "term" + std::to_string(i);
+ std::string termKind = pt.get<std::string>(currentTerm + ".kind");
+ boost::algorithm::to_lower(termKind);
+ int currentTermType = pt.get<int>(currentTerm + ".type");
+ std::string termName;
+ try
+ {
+ termName = pt.get<std::string>(currentTerm + ".name");
+ if (verbose)
+ std::cout << "Trying to add " + termName + " as term" << std::endl;
+ } catch (boost::property_tree::ptree_bad_path err)
+ {
+ termName = "Unnamed";
+ if (verbose)
+ {
+ std::cout << "Name field for " + currentTerm + " does not exist" << std::endl;
+ }
+ }
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term;
+
+ CT_LOADABLE_TERMS(SCALAR, SCALAR);
+
+ if (!term)
+ {
+ throw std::runtime_error("Term type \"" + termKind + "\" not supported");
+ }
+ else
+ {
+ addTerm(filename, currentTerm, currentTermType, term, this, verbose);
+ }
+ currentTerm = "term" + std::to_string(++i);
+ } while (pt.find(currentTerm) != pt.not_found());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ return this->evaluateIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ return this->evaluateTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ return this->stateDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ return this->stateDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ return this->stateSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ return this->stateSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ return this->controlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ return this->controlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ return this->controlSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ return this->controlSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ return this->stateControlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ return this->stateControlDerivativeTerminalBase();
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
new file mode 100644
index 0000000..2338fcf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
@@ -0,0 +1,96 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include "CostFunctionQuadratic.hpp"
+#include "utility/utilities.hpp"
+
+#include "term/TermLoadMacros.hpp"
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A cost function which contains only terms that have analytical derivatives
+ *
+ * This class provides functions to evaluate a cost function and computes its first
+ * and second order derivatives. This cost function assumes that analytical derivatives
+ * for all terms are available.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionAnalytical : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * \brief Basic constructor
+ */
+ CostFunctionAnalytical();
+
+ /**
+ * \brief Copy constructor
+ * @param arg cost function to copy
+ */
+ CostFunctionAnalytical(const CostFunctionAnalytical& arg);
+ /**
+ * \brief Constructor loading function from file
+ * @param filename config file location
+ * @param verbose flag enabling printouts
+ */
+ CostFunctionAnalytical(const std::string& filename, bool verbose = false);
+
+ /**
+ * Deep-cloning of cost function
+ * @return base pointer to clone
+ */
+ CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const;
+
+ /**
+ * Destructor
+ */
+ ~CostFunctionAnalytical();
+
+ SCALAR evaluateIntermediate() override;
+ SCALAR evaluateTerminal() override;
+
+ state_vector_t stateDerivativeIntermediate() override;
+ state_vector_t stateDerivativeTerminal() override;
+
+ state_matrix_t stateSecondDerivativeIntermediate() override;
+ state_matrix_t stateSecondDerivativeTerminal() override;
+
+ control_vector_t controlDerivativeIntermediate() override;
+ control_vector_t controlDerivativeTerminal() override;
+
+ control_matrix_t controlSecondDerivativeIntermediate() override;
+ control_matrix_t controlSecondDerivativeTerminal() override;
+
+ control_state_matrix_t stateControlDerivativeIntermediate() override;
+ control_state_matrix_t stateControlDerivativeTerminal() override;
+
+ void loadFromConfigFile(const std::string& filename, bool verbose = false) override;
+
+private:
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
new file mode 100644
index 0000000..46caf1c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
@@ -0,0 +1,493 @@
+/**********************************************************************************************************************
+ This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+ Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+ Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadratic()
+{
+ eps_ = sqrt(Eigen::NumTraits<SCALAR>::epsilon());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadratic(const CostFunctionQuadratic& arg)
+ : CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ eps_(arg.eps_),
+ doubleSidedDerivative_(arg.doubleSidedDerivative_)
+{
+ intermediateCostAnalytical_.resize(arg.intermediateCostAnalytical_.size());
+ finalCostAnalytical_.resize(arg.finalCostAnalytical_.size());
+
+ for (size_t i = 0; i < arg.intermediateCostAnalytical_.size(); i++)
+ {
+ intermediateCostAnalytical_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.intermediateCostAnalytical_[i]->clone());
+ }
+
+ for (size_t i = 0; i < arg.finalCostAnalytical_.size(); i++)
+ {
+ finalCostAnalytical_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.finalCostAnalytical_[i]->clone());
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionQuadratic()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ throw std::runtime_error("controlDerivativeTerminal() not implemented in CostFunctionQuadratic");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ throw std::runtime_error("controlSecondDerivativeTerminal() not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ throw std::runtime_error("stateControlDerivativeTerminal() not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::updateReferenceState(const state_vector_t& x_ref)
+{
+ for (auto costIntermediate : intermediateCostAnalytical_)
+ costIntermediate->updateReferenceState(x_ref);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::updateFinalState(const state_vector_t& x_final)
+{
+ for (auto costFinal : finalCostAnalytical_)
+ costFinal->updateReferenceState(x_final);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateTest()
+{
+ state_vector_t derivative = stateDerivativeIntermediate();
+ state_vector_t derivativeNd = stateDerivativeIntermediateNumDiff();
+ std::cout << "norm error between derivative/numdiff state : " << std::endl
+ << (derivative - derivativeNd).norm() << std::endl;
+
+ return (derivative.isApprox(derivativeNd, 1e-6));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateTest()
+{
+ control_vector_t derivative = controlDerivativeIntermediate();
+ control_vector_t derivativeNd = controlDerivativeIntermediateNumDiff();
+ std::cout << "norm error between derivative/numdiff control : " << std::endl
+ << (derivative - derivativeNd).norm() << std::endl;
+
+ return (derivative.isApprox(derivativeNd, 1e-6));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateTermById(const size_t id)
+{
+ return intermediateCostAnalytical_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalTermById(const size_t id)
+{
+ return finalCostAnalytical_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateTermByName(const std::string& name)
+{
+ for (auto term : intermediateCostAnalytical_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionQuadratic");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalTermByName(const std::string& name)
+{
+ for (auto term : finalCostAnalytical_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionQuadratic");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateNumDiff()
+{
+ state_vector_t dFdx = state_vector_t::Zero();
+ state_vector_t x_local;
+ control_vector_t u_local;
+ SCALAR t_local;
+ this->getCurrentStateAndControl(x_local, u_local, t_local);
+ SCALAR dxdt_ref = this->evaluateIntermediate();
+
+ for (size_t i = 0; i < STATE_DIM; ++i)
+ {
+ // inspired from http://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic
+ SCALAR h = eps_ * std::max(std::abs(x_local(i)), 1.0);
+ volatile SCALAR x_ph = x_local(i) + h;
+ SCALAR dxp = x_ph - x_local(i);
+
+ state_vector_t x_perturbed = x_local;
+ x_perturbed(i) = x_ph;
+
+ // get evaluation of f(x,u)
+ this->setCurrentStateAndControl(x_perturbed, u_local, t_local);
+ SCALAR dxdt = this->evaluateIntermediate();
+
+ if (doubleSidedDerivative_)
+ {
+ SCALAR dxdt_low;
+
+ volatile SCALAR x_mh = x_local(i) - h;
+ SCALAR dxm = x_local(i) - x_mh;
+
+ x_perturbed = x_local;
+ x_perturbed(i) = x_mh;
+ this->setCurrentStateAndControl(x_perturbed, u_local, t_local);
+ dxdt_low = this->evaluateIntermediate();
+ dFdx(i, 0) = (dxdt - dxdt_low) / (dxp + dxm);
+ }
+ else
+ {
+ dFdx(i, 0) = (dxdt - dxdt_ref) / dxp;
+ }
+ }
+
+ return dFdx;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateNumDiff()
+{
+ control_vector_t dFdu = control_vector_t::Zero();
+ state_vector_t x_local;
+ control_vector_t u_local;
+ SCALAR t_local;
+ this->getCurrentStateAndControl(x_local, u_local, t_local);
+ SCALAR dxdt_ref = this->evaluateIntermediate();
+
+ for (size_t i = 0; i < CONTROL_DIM; ++i)
+ {
+ // inspired from http://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic
+ SCALAR h = eps_ * std::max(std::abs(u_local(i)), 1.0);
+ volatile SCALAR u_ph = u_local(i) + h;
+ SCALAR dup = u_ph - u_local(i);
+
+ control_vector_t u_perturbed = u_local;
+ u_perturbed(i) = u_ph;
+
+ // get evaluation of f(x,u)
+ this->setCurrentStateAndControl(x_local, u_perturbed, t_local);
+ SCALAR dxdt = this->evaluateIntermediate();
+
+ if (doubleSidedDerivative_)
+ {
+ SCALAR dxdt_low;
+
+ volatile SCALAR u_mh = u_local(i) - h;
+ SCALAR dum = u_local(i) - u_mh;
+
+ u_perturbed = u_local;
+ u_perturbed(i) = u_mh;
+ this->setCurrentStateAndControl(x_local, u_perturbed, t_local);
+ dxdt_low = this->evaluateIntermediate();
+
+ dFdu(i, 0) = (dxdt - dxdt_low) / (dup + dum);
+ }
+ else
+ {
+ dFdu(i, 0) = (dxdt - dxdt_ref) / dup;
+ }
+ }
+
+ return dFdu;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ /*!
+ * do nothing at all
+ */
+}
+
+
+// add terms
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose)
+{
+ intermediateCostAnalytical_.push_back(term);
+ if (verbose)
+ {
+ std::string name = term->getName();
+ std::cout << "Trying to add term as intermediate" << std::endl;
+ }
+
+ return intermediateCostAnalytical_.size() - 1;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose)
+{
+ finalCostAnalytical_.push_back(term);
+ if (verbose)
+ {
+ std::string name = term->getName();
+ std::cout << "Trying to add term as final" << std::endl;
+ }
+
+ return finalCostAnalytical_.size() - 1;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateBase()
+{
+ SCALAR y = SCALAR(0.0);
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ y += it->computeActivation(this->t_) * it->eval(this->x_, this->u_, this->t_);
+ }
+
+ return y;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalBase()
+{
+ SCALAR y = SCALAR(0.0);
+
+ for (auto it : this->finalCostAnalytical_)
+ y += it->evaluate(this->x_, this->u_, this->t_);
+
+ return y;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateBase()
+{
+ state_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminalBase()
+{
+ state_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediateBase()
+{
+ state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateSecondDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminalBase()
+{
+ state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateSecondDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateBase()
+{
+ control_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->controlDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminalBase()
+{
+ control_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->controlDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+// get control second derivatives
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediateBase()
+{
+ control_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->controlSecondDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminalBase()
+{
+ control_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->controlSecondDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+// get state-control derivatives
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediateBase()
+{
+ control_state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateControlDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminalBase()
+{
+ control_state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateControlDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
new file mode 100644
index 0000000..51ba632
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
@@ -0,0 +1,239 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "CostFunction.hpp"
+#include "term/TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief Describes a cost function with a quadratic approximation, i.e. one that
+ * can compute first and second order derivatives with respect to state and
+ * control input. **This does NOT mean it has to be a purely quadratic cost
+ * function**. If you are looking for a purely quadratic cost function, check
+ * CostFunctionQuadraticSimple.
+ *
+ * A cost function is assumed to be a sum of intermediate and final terms, i.e.
+ * \f$ J(x,u,t) = \sum_{n=0}^{N_i} T_{i,n}(x,u,t) + \sum_{n=0}^{N_f} T_{i,f}(x,u,t) \f$
+ * These terms can have arbitrary form.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionQuadratic : public CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef CostFunction<STATE_DIM, CONTROL_DIM, SCALAR> BASE;
+
+ /**
+ * Constructor
+ */
+ CostFunctionQuadratic();
+
+ /**
+ * Copy constructor
+ * @param arg other cost function
+ */
+ CostFunctionQuadratic(const CostFunctionQuadratic& arg);
+
+ /**
+ * Clones the cost function.
+ * @return
+ */
+ virtual CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * Destructor
+ */
+ virtual ~CostFunctionQuadratic();
+
+ /**
+ * \brief Adds an intermediate term
+ * @param term intermediate term
+ * @param verbose verbosity flag which enables printout
+ * @return
+ */
+ virtual size_t addIntermediateTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose = false);
+
+ virtual void addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose = false);
+
+ /**
+ * \brief Adds a final term
+ * @param term final term
+ * @param verbose verbosity flag which enables printout
+ * @return
+ */
+ virtual size_t addFinalTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term, bool verbose = false);
+
+ virtual void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose = false);
+
+ /**
+ * \brief Loads cost function from config file
+ * @param filename config file location
+ * @param verbose verbosity flag which enables printout
+ */
+ virtual void loadFromConfigFile(const std::string& filename, bool verbose = false);
+
+ /**
+ * \brief Computes intermediate-cost first-order derivative with respect to state
+ * @return derivative vector (Jacobian)
+ */
+ virtual state_vector_t stateDerivativeIntermediate() = 0;
+
+ /**
+ * Computes terminal-cost first-order derivative with respect to state
+ * @return derivative vector (Jacobian)
+ */
+ virtual state_vector_t stateDerivativeTerminal() = 0;
+
+ /**
+ * \brief Computes intermediate-cost second-order derivative with respect to state
+ * @return derivative matrix (Jacobian)
+ */
+ virtual state_matrix_t stateSecondDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost second-order derivative with respect to state
+ * @return derivative matrix (Jacobian)
+ */
+ virtual state_matrix_t stateSecondDerivativeTerminal() = 0;
+
+ /**
+ * \brief Computes intermediate-cost first-order derivative with respect to control
+ * @return derivative vector (Jacobian)
+ */
+ virtual control_vector_t controlDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes terminal-cost first-order derivative with respect to control
+ *
+ * Not available for all cost functions. Throws an exception if not available.
+ * @return derivative vector (Jacobian)
+ */
+ virtual control_vector_t controlDerivativeTerminal();
+ /**
+ * \brief Computes intermediate-cost second-order derivative with respect to input
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_matrix_t controlSecondDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost second-order derivative with respect to input
+ *
+ * Not available for all cost functions. Throws an exception if not available.
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_matrix_t controlSecondDerivativeTerminal();
+
+ /**
+ * \brief Computes intermediate-cost derivative with respect to state and control
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_state_matrix_t stateControlDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost derivative with respect to state and control
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_state_matrix_t stateControlDerivativeTerminal();
+
+ virtual void updateReferenceState(const state_vector_t& x_ref);
+
+ virtual void updateFinalState(const state_vector_t& x_final);
+
+ //! compare the state derivative against numerical differentiation
+ bool stateDerivativeIntermediateTest();
+
+ //! compare the control derivative against numerical differentiation
+ bool controlDerivativeIntermediateTest();
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermByName(const std::string& name);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermByName(const std::string& name);
+
+ //! initialize the cost function (e.g. to be used in CostFunctionAD)
+ virtual void initialize();
+
+protected:
+ //! evaluate intermediate analytical cost terms
+ SCALAR evaluateIntermediateBase();
+
+ //! evaluate terminal analytical cost terms
+ SCALAR evaluateTerminalBase();
+
+ //! evaluate intermediate analytical state derivatives
+ state_vector_t stateDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical state derivatives
+ state_vector_t stateDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical state second derivatives
+ state_matrix_t stateSecondDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical state second derivatives
+ state_matrix_t stateSecondDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control derivatives
+ control_vector_t controlDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control derivatives
+ control_vector_t controlDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control second derivatives
+ control_matrix_t controlSecondDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control second derivatives
+ control_matrix_t controlSecondDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control mixed state control derivatives
+ control_state_matrix_t stateControlDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control mixed state control derivatives
+ control_state_matrix_t stateControlDerivativeTerminalBase();
+
+ //! compute the state derivative by numerical differentiation (can be used for testing)
+ state_vector_t stateDerivativeIntermediateNumDiff();
+
+ //! compute the control derivative by numerical differentiation (can be used for testing)
+ control_vector_t controlDerivativeIntermediateNumDiff();
+
+ //! stepsize for numerical differentiation
+ SCALAR eps_;
+
+ //! use double sided derivatives in numerical differentiation
+ bool doubleSidedDerivative_ = true;
+
+ /** list of intermediate cost terms for which analytic derivatives are available */
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> intermediateCostAnalytical_;
+
+ /** list of final cost terms for which analytic derivatives are available */
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> finalCostAnalytical_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
new file mode 100644
index 0000000..90a107d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -0,0 +1,157 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple()
+{
+ x_nominal_.setZero();
+ Q_.setZero();
+ u_nominal_.setZero();
+ R_.setZero();
+ x_final_.setZero();
+ Q_final_.setZero();
+ x_deviation_.setZero();
+ u_deviation_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_vector_t& x_nominal,
+ const control_vector_t& u_nominal,
+ const state_vector_t& x_final,
+ const state_matrix_t& Q_final)
+ : x_nominal_(x_nominal), Q_(Q), u_nominal_(u_nominal), R_(R), x_final_(x_final), Q_final_(Q_final)
+{
+ x_deviation_.setZero();
+ u_deviation_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionQuadraticSimple()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple(
+ const CostFunctionQuadraticSimple& arg)
+ : x_deviation_(arg.x_deviation_),
+ x_nominal_(arg.x_nominal_),
+ Q_(arg.Q_),
+ u_deviation_(arg.u_deviation_),
+ u_nominal_(arg.u_nominal_),
+ R_(arg.R_),
+ x_final_(arg.x_final_),
+ Q_final_(arg.Q_final_)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>*
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ this->x_ = x;
+ this->u_ = u;
+ this->t_ = t;
+
+ this->x_deviation_ = x - x_nominal_;
+ this->u_deviation_ = u - u_nominal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Q_ * x_deviation_)(0);
+ SCALAR costR = SCALAR(0.5) * (u_deviation_.transpose() * R_ * u_deviation_)(0);
+ return costQ + costR;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ return Q_ * x_deviation_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ return R_ * u_deviation_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ state_vector_t x_deviation_final = this->x_ - x_final_;
+ return SCALAR(0.5) * x_deviation_final.transpose() * Q_final_ * x_deviation_final;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ state_vector_t x_deviation_final = this->x_ - x_final_;
+ return Q_final_ * x_deviation_final;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ return Q_final_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updateReferenceState(const state_vector_t& x_ref)
+{
+ x_nominal_ = x_ref;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updateFinalState(const state_vector_t& x_final)
+{
+ x_final_ = x_final;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
new file mode 100644
index 0000000..05c1bc8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -0,0 +1,106 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "CostFunctionQuadratic.hpp"
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup CostFunction
+ *
+ * \brief A simple quadratic cost function
+ *
+ * A simple, purely-quadratic cost function of the form
+ * \f$ J(x,u,t) = \bar{x}^T Q \bar{x} + \bar{u}^T R \bar{u} + \bar{x}^T_f Q_f \bar{x}^T_f \f$
+ * where \f$ \bar{x}, \bar{u} \f$ indicate deviations from a nominal (desired) state and control
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionQuadraticSimple : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * Constructs a simple, purely quadratic cost function with all zero elements.
+ */
+ CostFunctionQuadraticSimple();
+ /**
+ * Constructs a simple, purely quadratic cost function
+ * @param Q intermediate state cost weighting
+ * @param R intermediate control cost weighting
+ * @param x_nominal nominal (desired) state
+ * @param u_nominal nominal (desired) control
+ * @param x_final nominal (desired) final state
+ * @param Q_final final state cost weighting
+ */
+ CostFunctionQuadraticSimple(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_vector_t& x_nominal,
+ const control_vector_t& u_nominal,
+ const state_vector_t& x_final,
+ const state_matrix_t& Q_final);
+
+ virtual ~CostFunctionQuadraticSimple();
+
+ CostFunctionQuadraticSimple(const CostFunctionQuadraticSimple& arg);
+
+ /**
+ * Clones the cost function.
+ * @return
+ */
+ CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t) override;
+
+ virtual SCALAR evaluateIntermediate() override;
+
+ virtual state_vector_t stateDerivativeIntermediate() override;
+
+ virtual state_matrix_t stateSecondDerivativeIntermediate() override;
+
+ virtual control_vector_t controlDerivativeIntermediate() override;
+
+ virtual control_matrix_t controlSecondDerivativeIntermediate() override;
+
+ virtual control_state_matrix_t stateControlDerivativeIntermediate() override;
+
+ virtual SCALAR evaluateTerminal() override;
+
+ virtual state_vector_t stateDerivativeTerminal() override;
+
+ virtual state_matrix_t stateSecondDerivativeTerminal() override;
+
+ virtual void updateReferenceState(const state_vector_t& x_ref) override;
+
+ virtual void updateFinalState(const state_vector_t& x_final) override;
+
+protected:
+ state_vector_t x_deviation_;
+ state_vector_t x_nominal_;
+ state_matrix_t Q_;
+
+ control_vector_t u_deviation_;
+ control_vector_t u_nominal_;
+ control_matrix_t R_;
+
+ state_vector_t x_final_;
+ state_matrix_t Q_final_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp
new file mode 100644
index 0000000..5bdead8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp
@@ -0,0 +1,21 @@
+/*!
+ * a convenience include file collecting all class implementations related to cost functions
+ */
+
+#pragma once
+
+// terms
+#include "term/TermBase-impl.hpp"
+#include "term/TermLinear-impl.hpp"
+#include "term/TermMixed-impl.hpp"
+#include "term/TermQuadratic-impl.hpp"
+#include "term/TermQuadMult-impl.hpp"
+#include "term/TermQuadTracking-impl.hpp"
+#include "term/TermStateBarrier-impl.hpp"
+
+// costfunctions
+#include "CostFunction-impl.hpp"
+#include "CostFunctionAD-impl.hpp"
+#include "CostFunctionAnalytical-impl.hpp"
+#include "CostFunctionQuadratic-impl.hpp"
+#include "CostFunctionQuadraticSimple-impl.hpp"
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp b/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp
new file mode 100644
index 0000000..1c8a194
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp
@@ -0,0 +1,21 @@
+/*!
+ * a convenience include file collecting all class declarations related to cost functions
+ */
+
+#pragma once
+
+// time activations
+#include "term/TermBase.hpp"
+#include "term/TermLinear.hpp"
+#include "term/TermMixed.hpp"
+#include "term/TermQuadratic.hpp"
+#include "term/TermQuadMult.hpp"
+#include "term/TermQuadTracking.hpp"
+#include "term/TermStateBarrier.hpp"
+
+// costfunctions
+#include "CostFunction.hpp"
+#include "CostFunctionQuadratic.hpp"
+#include "CostFunctionAD.hpp"
+#include "CostFunctionAnalytical.hpp"
+#include "CostFunctionQuadraticSimple.hpp"
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp
new file mode 100644
index 0000000..b87cbb7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermBase(std::string name)
+ : name_(name),
+ c_i_(
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>>(new ct::core::tpl::ActivationBase<SCALAR_EVAL>()))
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermBase(const TermBase& arg) : name_(arg.name_), c_i_(arg.c_i_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermBase()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR_EVAL TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::eval(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& u,
+ const SCALAR_EVAL& t)
+{
+ return computeActivation(t) * evaluate(x, u, t);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ throw std::runtime_error("The cost function term term " + name_ + " does not implement evaluate CppadCg.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+bool TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::isActiveAtTime(SCALAR_EVAL t)
+{
+ return c_i_->isActive(t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR_EVAL TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::computeActivation(SCALAR_EVAL t)
+{
+ return c_i_->computeActivation(t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function or "
+ "implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setTimeActivation(
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i,
+ bool verbose)
+{
+ c_i_ = c_i;
+ if (verbose)
+ c_i_->printInfo();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadTimeActivation(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ if (verbose)
+ std::cout << "TermBase: loading TimeActivation ..." << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ std::string activationKind = pt.get<std::string>(termName + ".time_activation" + ".kind");
+ boost::algorithm::to_lower(activationKind);
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i;
+ CT_LOADABLE_ACTIVATIONS(SCALAR_EVAL);
+ c_i->loadConfigFile(filename, termName + ".time_activation", verbose);
+ if (!c_i)
+ {
+ throw std::runtime_error("Activation type \"" + activationKind + "\" not supported");
+ }
+ else
+ {
+ c_i_ = c_i;
+ if (verbose)
+ c_i_->printInfo();
+ }
+ } catch (std::exception& e)
+ {
+ return;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const std::string& TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getName() const
+{
+ return name_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setName(const std::string& termName)
+{
+ name_ = termName;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceControl(
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getReferenceState()
+ const
+{
+ throw std::runtime_error("getReferenceState is not implemented for the current term!");
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp
new file mode 100644
index 0000000..8dd6bbf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp
@@ -0,0 +1,181 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <boost/algorithm/string.hpp>
+
+#include <ct/core/common/activations/Activations.h>
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup CostFunction
+ *
+ * \brief An interface for a term, supporting both analytical and auto-diff terms
+ *
+ * Derive from this term to implement your own term. You only have to implement
+ * evaluateCppadCg() if you want to use auto-diff. Otherwise, you need to implement
+ * evaluate() as well as the derivatives. In case you want to go for the most general
+ * implementation, you can implement a local, templated evalLocal() method in derived terms,
+ * which gets called by evaluate() and evaluateCppadCg(), see e.g. TermLinear.
+ *
+ * An example for an implementation of a custom term is given in \ref TermQuadratic.hpp
+ **/
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermBase
+{
+protected:
+ //! a name identifier for this term
+ std::string name_;
+ //! time activations for this term
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i_;
+
+public:
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ /**
+ * \brief Default constructor
+ * @param name Name of the term
+ */
+ TermBase(std::string name = "Unnamed");
+
+ /**
+ * \brief Copy Cunstructor
+ * @param arg The term to copy
+ */
+ TermBase(const TermBase& arg);
+
+ /**
+ * \brief Deep-copy term
+ * @return
+ */
+ virtual TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const = 0;
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~TermBase();
+
+ /**
+ * @brief Evaluates the term at x, u, t
+ *
+ * @param[in] x The current state
+ * @param[in] u The current control
+ * @param[in] t The current time
+ *
+ * @return The evaluatated cost term
+ */
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) = 0;
+
+ /**
+ * @brief The evaluate method used for jit compilation in CostfunctionAD
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The evaluated cost
+ */
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t);
+
+ /**
+ * @brief Gets called by the analytical costfunction. Adds time
+ * dependent activations on top of the term
+ *
+ * @param[in] x The current state
+ * @param[in] u The current control
+ * @param[in] t The current time
+ *
+ * @return The evaluatated cost term
+ */
+ SCALAR_EVAL eval(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& u,
+ const SCALAR_EVAL& t);
+
+ /**
+ * \brief Returns if term is non-zero at a specific time
+ * By default, all terms are evaluated at all times. However, if a term is not active at a certain time, you can overload this
+ * function to spare evaluations of the term and its derivatives
+ * @param t time
+ * @return true if term is active at t
+ */
+ virtual bool isActiveAtTime(SCALAR_EVAL t);
+
+ //! compute time activation
+ SCALAR_EVAL computeActivation(SCALAR_EVAL t);
+
+ //! compute derivative of this cost term w.r.t. the state
+ virtual core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute second order derivative of this cost term w.r.t. the state
+ virtual state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute derivative of this cost term w.r.t. the control input
+ virtual core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute second order derivative of this cost term w.r.t. the control input
+ virtual control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute the cross-term derivative (state-control) of this cost function term
+ virtual control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! load this term from a configuration file
+ virtual void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false);
+
+ //! set the time activation functions for this term
+ void setTimeActivation(std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i, bool verbose = false);
+
+ //! load the time activation functions for this term from file
+ void loadTimeActivation(const std::string& filename, const std::string& termName, bool verbose = false);
+
+ /**
+ * \brief Returns the name of the term
+ * @param termName name of the term
+ */
+ const std::string& getName() const;
+
+ /**
+ * \brief Sets the name of the term
+ * @param termName
+ */
+ void setName(const std::string& termName);
+
+ //! updates the reference state for this term
+ virtual void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState);
+
+ //! updates the reference control for this term
+ virtual void updateReferenceControl(const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl);
+
+ //! retrieve this term's current reference state
+ virtual Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> getReferenceState() const;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp
new file mode 100644
index 0000000..aaa540e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear(const core::StateVector<STATE_DIM, SCALAR_EVAL> a,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b,
+ const SCALAR_EVAL c)
+ : a_(a), b_(b), c_(c)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear(const TermLinear &arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg), a_(arg.a_), b_(arg.b_), c_(arg.c_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+ *TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermLinear()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR &t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar> &x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar> &u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return a_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return b_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return control_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string &filename,
+ const std::string &termName,
+ bool verbose)
+{
+ // read in the file and put the valus in a_ and b_
+ loadMatrixCF(filename, "a", a_, termName);
+ loadMatrixCF(filename, "b", b_, termName);
+ if (verbose)
+ {
+ std::cout << "Read a as a= \n" << a_ << std::endl;
+ std::cout << "Read b as b= \n" << b_ << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp
new file mode 100644
index 0000000..5011ab5
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp
@@ -0,0 +1,100 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A linear term of type \f$ J = a x + b u + c \f$
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermLinear : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermLinear(const core::StateVector<STATE_DIM, SCALAR_EVAL> a,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b,
+ const SCALAR_EVAL c = 0.);
+
+ TermLinear();
+
+ TermLinear(const TermLinear &arg);
+
+ TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR> *clone() const override;
+
+ virtual ~TermLinear();
+
+ SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR &t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar> &x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar> &u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ void loadConfigFile(const std::string &filename,
+ const std::string &termName,
+ bool verbose = false) override; // virtual function for data loading
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1> &x, const Eigen::Matrix<SC, CONTROL_DIM, 1> &u, const SC &t);
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> a_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b_;
+ SCALAR_EVAL c_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1> &u,
+ const SC &t)
+{
+ Eigen::Matrix<SC, 1, 1> y_eigen = a_.template cast<SC>().transpose() * x + b_.template cast<SC>().transpose() * u;
+ SC y = y_eigen(0, 0) + SC(c_);
+ return y;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp
new file mode 100644
index 0000000..a637d75
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp
@@ -0,0 +1,19 @@
+
+#pragma once
+
+#include "TermLinear.hpp"
+#include "TermQuadratic.hpp"
+#include "TermQuadMult.hpp"
+
+#define CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TERM, TERMNAME) \
+ if (termKind == TERMNAME) \
+ { \
+ term = std::shared_ptr<TERM<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>>( \
+ new TERM<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>()); \
+ term->setName(TERMNAME); \
+ }
+
+#define CT_LOADABLE_TERMS(SCALAR_EVAL, SCALAR) \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermLinear, "linear") \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermQuadratic, "quadratic") \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermQuadMult, "quadratic-multiplicative")
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp
new file mode 100644
index 0000000..98d6093
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp
@@ -0,0 +1,161 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(const control_state_matrix_t& P) : P_(P)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed()
+{
+ P_.setZero(); // default values
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(const control_state_matrix_t& P,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : P_(P), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(
+ const TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg), P_(arg.P_), x_ref_(arg.x_ref_), u_ref_(arg.u_ref_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone()
+ const
+{
+ return new TermMixed(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermMixed()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const control_state_matrix_double_t& P)
+{
+ P_ = P.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM>& x_ref,
+ const core::ControlVector<CONTROL_DIM>& u_ref)
+{
+ x_ref_ = x_ref.template cast<SCALAR_EVAL>();
+ u_ref_ = u_ref.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL> TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return P_.transpose() * uDiff;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL> TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ return P_ * xDiff;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return P_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "P", P_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read P as P = \n" << P_ << std::endl;
+ std::cout << "Read x_ref as x_ref = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_ref as u_ref = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+ x_ref_ = newRefState;
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp
new file mode 100644
index 0000000..4f44eca
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp
@@ -0,0 +1,112 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A basic quadratic term of type \f$ J = u^T P x \f$
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermMixed : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermMixed();
+
+ TermMixed(const control_state_matrix_t& P);
+
+ TermMixed(const control_state_matrix_t& P,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermMixed(const TermMixed& arg);
+
+ virtual ~TermMixed();
+
+ TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const control_state_matrix_double_t& P);
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM>& x_ref,
+ const core::ControlVector<CONTROL_DIM>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+ void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState) override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ control_state_matrix_t P_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (uDiff.transpose() * P_.template cast<SC>() * xDiff)(0, 0);
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp
new file mode 100644
index 0000000..c2bb31d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp
@@ -0,0 +1,188 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R)
+ : Q_(Q), R_(R)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult()
+{
+ Q_.setIdentity(); // default values
+ R_.setIdentity();
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : Q_(Q), R_(R), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const TermQuadMult& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_ref_(arg.x_ref_),
+ u_ref_(arg.u_ref_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadMult(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadMult()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const state_matrix_double_t& Q,
+ const control_matrix_double_t& R)
+{
+ Q_ = Q.template cast<SCALAR_EVAL>();
+ R_ = R.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM>& x_ref,
+ core::ControlVector<CONTROL_DIM>& u_ref)
+{
+ x_ref_ = x_ref.template cast<SCALAR_EVAL>();
+ ;
+ u_ref_ = u_ref.template cast<SCALAR_EVAL>();
+ ;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL r = (uDiff.transpose() * R_ * uDiff)(0, 0);
+
+ return (xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_) * r;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL r = (uDiff.transpose() * R_ * uDiff)(0, 0);
+
+ return (Q_ + Q_.transpose()) * r;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL q = (xDiff.transpose() * Q_ * xDiff)(0, 0);
+
+ return (uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_) * q;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ SCALAR_EVAL q = (xDiff.transpose() * Q_ * xDiff)(0, 0);
+
+ return (R_ + R_.transpose()) * q;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return (uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_).transpose() *
+ (xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ std::cout << "Read x_ref as x_ref = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_ref as u_ref = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp
new file mode 100644
index 0000000..f7f68a0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp
@@ -0,0 +1,114 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A multiplicative term of type \f$ J = (x^T Q x) (u^T R u) \f$
+ *
+ * Probably this term is not very useful but we use it for testing.
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadMult : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadMult();
+
+ TermQuadMult(const state_matrix_t& Q, const control_matrix_t& R);
+
+ TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermQuadMult(const TermQuadMult& arg);
+
+ virtual ~TermQuadMult();
+
+ TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const state_matrix_double_t& Q, const control_matrix_double_t& R);
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM>& x_ref,
+ core::ControlVector<CONTROL_DIM>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false);
+
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff)(0, 0) *
+ (uDiff.transpose() * R_.template cast<SC>() * uDiff)(0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp
new file mode 100644
index 0000000..c9cdeb7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp
@@ -0,0 +1,160 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::InterpolationType& stateSplineType,
+ const core::InterpolationType& controlSplineType,
+ const bool trackControlTrajectory)
+ : Q_(Q),
+ R_(R),
+ x_traj_ref_(stateSplineType),
+ u_traj_ref_(controlSplineType),
+ trackControlTrajectory_(trackControlTrajectory)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking()
+{
+ // default values
+ Q_.setIdentity();
+ R_.setIdentity();
+ trackControlTrajectory_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking(
+ const TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_traj_ref_(arg.x_traj_ref_),
+ u_traj_ref_(arg.u_traj_ref_),
+ trackControlTrajectory_(arg.trackControlTrajectory_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadTracking()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadTracking(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const state_matrix_double_t& Q,
+ const control_matrix_double_t& R)
+{
+ Q_ = Q.template cast<SCALAR_EVAL>();
+ R_ = R.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateTrajectory<STATE_DIM>& xTraj,
+ const core::ControlTrajectory<CONTROL_DIM>& uTraj)
+{
+ x_traj_ref_ = xTraj;
+ u_traj_ref_ = uTraj;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> xDiff = x - x_traj_ref_.eval(t);
+
+ return xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return Q_ + Q_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1> uDiff;
+
+ if (trackControlTrajectory_)
+ uDiff = u - u_traj_ref_.eval(t);
+ else
+ uDiff = u;
+
+ return uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return R_ + R_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp
new file mode 100644
index 0000000..2a2fb5c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp
@@ -0,0 +1,116 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A quadratic tracking term of type \f$ J(t) = (x_{ref}(t) - x)^T Q (x_{ref}(t) - x) + (u_{ref}(t) - u)^T R (u_{ref}(t) - u) \f$
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadTracking : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadTracking();
+
+ TermQuadTracking(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::InterpolationType& stateSplineType,
+ const core::InterpolationType& controlSplineType,
+ const bool trackControlTrajectory = false);
+
+ TermQuadTracking(const TermQuadTracking& arg);
+
+ virtual ~TermQuadTracking();
+
+ TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const state_matrix_double_t& Q, const control_matrix_double_t& R);
+
+ void setStateAndControlReference(const core::StateTrajectory<STATE_DIM>& xTraj,
+ const core::ControlTrajectory<CONTROL_DIM>& uTraj);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ // the reference trajectories to be tracked
+ ct::core::StateTrajectory<STATE_DIM, SCALAR_EVAL> x_traj_ref_;
+ ct::core::ControlTrajectory<CONTROL_DIM, SCALAR_EVAL> u_traj_ref_;
+
+ // Option whether the control trajectory deviation shall be penalized or not
+ bool trackControlTrajectory_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = x - x_traj_ref_.eval((SCALAR_EVAL)t).template cast<SC>();
+
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff;
+
+ if (trackControlTrajectory_)
+ uDiff = u - u_traj_ref_.eval((SCALAR_EVAL)t).template cast<SC>();
+ else
+ uDiff = u;
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff + uDiff.transpose() * R_.template cast<SC>() * uDiff)(
+ 0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp
new file mode 100644
index 0000000..072459c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp
@@ -0,0 +1,235 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R)
+ : Q_(Q), R_(R)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic()
+{
+ Q_.setConstant(9999); // default values
+ R_.setConstant(9999);
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : Q_(Q), R_(R), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(
+ const TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_ref_(arg.x_ref_),
+ u_ref_(arg.u_ref_)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadratic()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadratic(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>& Q,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>& R)
+{
+ Q_ = Q;
+ R_ = R;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getStateWeight() const
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getStateWeight()
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getControlWeight() const
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getControlWeight()
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+{
+ x_ref_ = x_ref;
+ u_ref_ = u_ref;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ return xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return Q_ + Q_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return R_ + R_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ boost::property_tree::ptree pt;
+ try
+ {
+ boost::property_tree::read_info(filename, pt);
+ } catch (...)
+ {
+ }
+ this->name_ = pt.get<std::string>(termName + ".name.", termName);
+
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ std::cout << "Read x_des as x_des = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_des as u_des = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+ x_ref_ = newRefState;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceControl(
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl)
+{
+ u_ref_ = newRefControl;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getReferenceState()
+ const
+{
+ return x_ref_;
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp
new file mode 100644
index 0000000..9607f47
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp
@@ -0,0 +1,128 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A basic quadratic term of type \f$ J = x^T Q x + u^T R u \f$
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadratic : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadratic();
+
+ TermQuadratic(const state_matrix_t& Q, const control_matrix_t& R);
+
+ TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermQuadratic(const TermQuadratic& arg);
+
+ virtual ~TermQuadratic();
+
+ virtual TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>& Q,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>& R);
+
+ const state_matrix_t& getStateWeight() const;
+
+ state_matrix_t& getStateWeight();
+
+ const control_matrix_t& getControlWeight() const;
+
+ control_matrix_t& getControlWeight();
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+ virtual void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState) override;
+
+ virtual void updateReferenceControl(const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl) override;
+
+ virtual Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> getReferenceState() const override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff + uDiff.transpose() * R_.template cast<SC>() * uDiff)(
+ 0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp
new file mode 100644
index 0000000..577ef59
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier(const state_vector_t& ub,
+ const state_vector_t& lb,
+ const state_vector_t& alpha)
+ : ub_(ub), lb_(lb), alpha_(alpha)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier()
+ : ub_(state_vector_t::Zero()), lb_(state_vector_t::Zero()), alpha_(state_vector_t::Zero())
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier(const TermStateBarrier& arg)
+ : ub_(arg.ub_), lb_(arg.lb_), alpha_(arg.alpha_)
+{
+ initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermStateBarrier()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermStateBarrier(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::initialize()
+{
+ barriers_.clear();
+ for (size_t i = 0; i < STATE_DIM; i++)
+ {
+ barriers_.push_back(ct::core::tpl::BarrierActivation<SCALAR>(ub_(i), lb_(i), alpha_(i)));
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ SCALAR c = SCALAR(0.0);
+ for (size_t i = 0; i < STATE_DIM; i++)
+ c += barriers_[i].computeActivation(x(i));
+ return c;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ ct::core::ADCGScalar c = ct::core::ADCGScalar(0.0);
+ for (size_t i = 0; i < STATE_DIM; i++)
+ c += barriers_[i].computeActivation(x(i));
+ return c;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ state_matrix_t Alpha;
+ state_matrix_t Ub;
+ state_matrix_t Lb;
+
+ loadMatrixCF(filename, "alpha", Alpha, termName);
+ loadMatrixCF(filename, "upper_bound", Ub, termName);
+ loadMatrixCF(filename, "lower_bound", Lb, termName);
+
+ alpha_ = Alpha.diagonal();
+ ub_ = Ub.diagonal();
+ lb_ = Lb.diagonal();
+
+ if (verbose)
+ {
+ std::cout << "Read alpha as = \n" << alpha_.transpose() << std::endl;
+ std::cout << "Read upper_bound as = \n" << ub_.transpose() << std::endl;
+ std::cout << "Read lower_bound as = \n" << lb_.transpose() << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp
new file mode 100644
index 0000000..ff9cff2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp
@@ -0,0 +1,75 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+#include <ct/core/common/activations/Activations.h>
+#include <ct/core/internal/traits/TraitSelectorSpecs.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A state barrier term (could also be considered a soft constraint)
+ * Note that this term explicitly excludes controls, as there are better
+ * ways to limit control effort in a "soft" way, e.g. through the use
+ * of sigmoid functions.
+ *
+ * @todo implement sigmoid barriers
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermStateBarrier : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermStateBarrier();
+
+ TermStateBarrier(const state_vector_t& ub, const state_vector_t& lb, const state_vector_t& alpha);
+
+ TermStateBarrier(const TermStateBarrier& arg);
+
+ virtual ~TermStateBarrier();
+
+ TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ //! load the term from config file, where the bounds are stored as matrices
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+protected:
+ void initialize();
+
+ state_vector_t alpha_;
+ state_vector_t ub_;
+ state_vector_t lb_;
+
+ std::vector<ct::core::tpl::BarrierActivation<SCALAR, ct::core::tpl::TraitSelector<SCALAR>>> barriers_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp b/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp
new file mode 100644
index 0000000..0147b31
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp
@@ -0,0 +1,146 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+
+#include <ct/optcon/costfunction/term/TermBase.hpp>
+
+#include "../CostFunction.hpp"
+
+namespace ct {
+namespace optcon {
+
+template <typename SCALAR>
+void loadScalarCF(const std::string& filename,
+ const std::string& scalarName,
+ SCALAR& scalar,
+ const std::string& termName = "")
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ scalar = pt.get<SCALAR>(termName + ".weights." + scalarName);
+}
+
+template <typename SCALAR>
+void loadScalarOptionalCF(const std::string& filename,
+ const std::string& scalarName,
+ SCALAR& scalar,
+ const std::string& termName,
+ const SCALAR& defaultValue)
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ scalar = pt.get<SCALAR>(termName + ".weights." + scalarName, defaultValue);
+}
+
+template <typename SCALAR, int ROW, int COL>
+void loadMatrixCF(const std::string& filename,
+ const std::string& matrixName,
+ Eigen::Matrix<SCALAR, ROW, COL>& matrix,
+ const std::string& termName = "")
+{
+ size_t rows = matrix.rows();
+ size_t cols = matrix.cols();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ double scaling = pt.get<double>(termName + ".weights." + matrixName + ".scaling", 1);
+
+ for (size_t i = 0; i < rows; i++)
+ {
+ for (size_t j = 0; j < cols; j++)
+ {
+ if (termName == "")
+ {
+ matrix(i, j) =
+ scaling *
+ pt.get<double>(matrixName + "." + "(" + std::to_string(i) + "," + std::to_string(j) + ")", 0.0);
+ }
+ else
+ {
+ matrix(i, j) = scaling *
+ pt.get<double>(termName + ".weights." + matrixName + "." + "(" + std::to_string(i) +
+ "," + std::to_string(j) + ")",
+ 0.0);
+ }
+ }
+ }
+}
+
+template <typename TERM_PTR, typename costFuncType>
+void addTerm(const std::string& filename,
+ std::string& currentTerm,
+ int currentTermType,
+ TERM_PTR term,
+ costFuncType* costFunc,
+ bool verbose = false)
+{
+ switch (currentTermType)
+ {
+ case 0:
+ costFunc->addIntermediateTerm(term, verbose);
+ break;
+ case 1:
+ costFunc->addFinalTerm(term, verbose);
+ break;
+ default:
+ if (verbose)
+ {
+ std::cout << "error code 1 => term type other than term0 and term1 encountered" << std::endl;
+ }
+ BOOST_PROPERTY_TREE_THROW(boost::property_tree::info_parser::info_parser_error(
+ "read error code = ", "", 1)); //error code 1 => term type otherthan term0 and term1 encountered
+ break;
+ }
+ term->loadTimeActivation(filename, currentTerm, verbose);
+ term->loadConfigFile(filename, currentTerm, verbose);
+
+ if (verbose)
+ std::cout << "Successfully loaded term " + currentTerm << std::endl;
+}
+
+template <typename TERM_PTR, typename costFuncType>
+void addADTerm(const std::string& filename,
+ std::string& currentTerm,
+ int currentTermType,
+ TERM_PTR term,
+ costFuncType* costFunc,
+ bool verbose = false)
+{
+ switch (currentTermType)
+ {
+ case 0:
+ costFunc->addIntermediateADTerm(term, verbose);
+ break;
+ case 1:
+ costFunc->addFinalADTerm(term, verbose);
+ break;
+ default:
+ if (verbose)
+ {
+ std::cout << "error code 1 => term type other than term0 and term1 encountered" << std::endl;
+ }
+ BOOST_PROPERTY_TREE_THROW(boost::property_tree::info_parser::info_parser_error(
+ "read error code = ", "", 1)); //error code 1 => term type otherthan term0 and term1 encountered
+ break;
+ }
+ term->loadTimeActivation(filename, currentTerm, verbose);
+ term->loadConfigFile(filename, currentTerm, verbose);
+
+ if (verbose)
+ std::cout << "Successfully loaded term " + currentTerm << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/Constraints b/ct_optcon/include/ct/optcon/dms/Constraints
new file mode 100644
index 0000000..825e219
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/Constraints
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "constraints/ConstraintDiscretizer.h"
+#include "constraints/ConstraintsContainerDms.h"
+#include "constraints/ContinuityConstraint.h"
+#include "constraints/InitStateConstraint.h"
+#include "constraints/TimeHorizonEqualityConstraint.h"
diff --git a/ct_optcon/include/ct/optcon/dms/Dms b/ct_optcon/include/ct/optcon/dms/Dms
new file mode 100644
index 0000000..9913e2c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/Dms
@@ -0,0 +1,18 @@
+
+#pragma once
+
+#include "dms_core/DmsSettings.h"
+#include "dms_core/DmsSolver.h"
+
+#include "dms_core/cost_evaluator/CostEvaluatorFull.h"
+#include "dms_core/cost_evaluator/CostEvaluatorSimple.h"
+
+#include "dms_core/spline/Linear/LinearSpliner.h"
+#include "dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h"
+
+#include "dms_core/ControllerDms.h"
+#include "dms_core/DmsDimensions.h"
+#include "dms_core/OptVectorDms.h"
+#include "dms_core/RKnDerivatives.h"
+#include "dms_core/ShotContainer.h"
+#include "dms_core/TimeGrid.h"
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h
new file mode 100644
index 0000000..dd154b4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h
@@ -0,0 +1,362 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/constraint/ConstraintContainerAD.h>
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The class takes continuous constraints defined with the
+ * constraint toolbox and discretizes them over the DMS shots. These
+ * discretized constraints can then be used in the NLP module
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintDiscretizer : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+
+ typedef ct::core::tpl::TimeArray<SCALAR> time_array_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ ConstraintDiscretizer() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintDiscretizer() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] c_continuous The continuous constraints
+ * @param[in] activeInd A vector defining at which shots the constraint
+ * is active
+ */
+ ConstraintDiscretizer(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ size_t N)
+ : w_(w),
+ controlSpliner_(controlSpliner),
+ timeGrid_(timeGrid),
+ N_(N),
+ constraintsCount_(0),
+ constraintsIntermediateCount_(0),
+ constraintsTerminalCount_(0),
+ nonZeroJacCount_(0),
+ nonZeroJacCountIntermediate_(0),
+ nonZeroJacCountTerminal_(0)
+ {
+ }
+
+
+ void setBoxConstraints(std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints)
+ {
+ constraints_.push_back(boxConstraints);
+ constraintsIntermediateCount_ += (N_ + 1) * boxConstraints->getIntermediateConstraintsCount();
+ constraintsTerminalCount_ += boxConstraints->getTerminalConstraintsCount();
+ constraintsCount_ = constraintsIntermediateCount_ + constraintsTerminalCount_;
+
+ discreteConstraints_.resize(constraintsCount_);
+ discreteLowerBound_.resize(constraintsCount_);
+ discreteUpperBound_.resize(constraintsCount_);
+
+ nonZeroJacCountIntermediate_ += (N_ + 1) * (boxConstraints->getJacobianStateNonZeroCountIntermediate() +
+ boxConstraints->getJacobianInputNonZeroCountIntermediate());
+ nonZeroJacCountTerminal_ += boxConstraints->getJacobianStateNonZeroCountTerminal() +
+ boxConstraints->getJacobianInputNonZeroCountTerminal();
+ nonZeroJacCount_ = nonZeroJacCountIntermediate_ + nonZeroJacCountTerminal_;
+
+ discreteJac_.resize(nonZeroJacCount_);
+ discreteIRow_.resize(nonZeroJacCount_);
+ discreteJCol_.resize(nonZeroJacCount_);
+ }
+
+ void setGeneralConstraints(
+ std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints)
+ {
+ constraints_.push_back(generalConstraints);
+ constraintsIntermediateCount_ += (N_ + 1) * generalConstraints->getIntermediateConstraintsCount();
+ constraintsTerminalCount_ += generalConstraints->getTerminalConstraintsCount();
+ constraintsCount_ = constraintsIntermediateCount_ + constraintsTerminalCount_;
+
+ discreteConstraints_.resize(constraintsCount_);
+ discreteLowerBound_.resize(constraintsCount_);
+ discreteUpperBound_.resize(constraintsCount_);
+
+ nonZeroJacCountIntermediate_ += (N_ + 1) * generalConstraints->getJacobianStateNonZeroCountIntermediate();
+ nonZeroJacCountTerminal_ += generalConstraints->getJacobianStateNonZeroCountTerminal();
+ nonZeroJacCount_ = nonZeroJacCountIntermediate_ + nonZeroJacCountTerminal_;
+
+ discreteJac_.resize(nonZeroJacCount_);
+ discreteIRow_.resize(nonZeroJacCount_);
+ discreteJCol_.resize(nonZeroJacCount_);
+ }
+
+ virtual VectorXs eval() override
+ {
+ size_t constraintSize = 0;
+ size_t discreteInd = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ SCALAR tShot = timeGrid_->getShotStartTime(n);
+ for (auto constraint : constraints_)
+ {
+ constraint->setCurrentStateAndControl(
+ w_->getOptimizedState(n), controlSpliner_->evalSpline(tShot, n), tShot);
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteConstraints_.segment(discreteInd, constraintSize) = constraint->evaluateIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteConstraints_.segment(discreteInd, constraintSize) = constraint->evaluateTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteConstraints_;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ size_t jacSize = 0;
+ size_t discreteInd = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ SCALAR tShot = timeGrid_->getShotStartTime(n);
+
+ for (auto constraint : constraints_)
+ {
+ constraint->setCurrentStateAndControl(
+ w_->getOptimizedState(n), controlSpliner_->evalSpline(tShot, n), tShot);
+ jacSize = constraint->getJacobianStateNonZeroCountIntermediate();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianStateSparseIntermediate();
+ discreteInd += jacSize;
+ }
+
+ jacSize = constraint->getJacobianInputNonZeroCountIntermediate();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianInputSparseIntermediate();
+ discreteInd += jacSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ jacSize = constraint->getJacobianStateNonZeroCountTerminal();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianStateSparseTerminal();
+ discreteInd += jacSize;
+ }
+ jacSize = constraint->getJacobianInputNonZeroCountTerminal();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianInputSparseTerminal();
+ discreteInd += jacSize;
+ }
+ }
+
+ return discreteJac_;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override { return nonZeroJacCount_; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t discreteInd = 0;
+ size_t rowOffset = 0;
+ size_t nnEle = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ nnEle = constraint->getJacobianStateNonZeroCountIntermediate();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowStateIntermediate(constraint->getJacobianStateNonZeroCountIntermediate());
+ Eigen::VectorXi jColStateIntermediate(constraint->getJacobianStateNonZeroCountIntermediate());
+ constraint->sparsityPatternStateIntermediate(iRowStateIntermediate, jColStateIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowStateIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColStateIntermediate.array() + w_->getStateIndex(n);
+ discreteInd += nnEle;
+ }
+
+ nnEle = constraint->getJacobianInputNonZeroCountIntermediate();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowInputIntermediate(constraint->getJacobianInputNonZeroCountIntermediate());
+ Eigen::VectorXi jColInputIntermediate(constraint->getJacobianInputNonZeroCountIntermediate());
+ constraint->sparsityPatternInputIntermediate(iRowInputIntermediate, jColInputIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowInputIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColInputIntermediate.array() + w_->getControlIndex(n);
+ discreteInd += nnEle;
+ }
+
+ rowOffset += constraint->getJacobianStateNonZeroCountIntermediate() +
+ constraint->getJacobianInputNonZeroCountIntermediate();
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ nnEle = constraint->getJacobianStateNonZeroCountTerminal();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowStateIntermediate(constraint->getJacobianStateNonZeroCountTerminal());
+ Eigen::VectorXi jColStateIntermediate(constraint->getJacobianStateNonZeroCountTerminal());
+ constraint->sparsityPatternStateTerminal(iRowStateIntermediate, jColStateIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowStateIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColStateIntermediate.array() + w_->getStateIndex(N_);
+ discreteInd += nnEle;
+ }
+
+ nnEle = constraint->getJacobianInputNonZeroCountTerminal();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowInputIntermediate(constraint->getJacobianInputNonZeroCountTerminal());
+ Eigen::VectorXi jColInputIntermediate(constraint->getJacobianInputNonZeroCountTerminal());
+ constraint->sparsityPatternInputTerminal(iRowInputIntermediate, jColInputIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowInputIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColInputIntermediate.array() + w_->getControlIndex(N_);
+ discreteInd += nnEle;
+ }
+ rowOffset +=
+ constraint->getJacobianStateNonZeroCountTerminal() + constraint->getJacobianInputNonZeroCountTerminal();
+ }
+
+ iRow_vec = discreteIRow_;
+ jCol_vec = discreteJCol_;
+ }
+
+ virtual VectorXs getLowerBound() override
+ {
+ size_t discreteInd = 0;
+ size_t constraintSize = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteLowerBound_.segment(discreteInd, constraintSize) = constraint->getLowerBoundsIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteLowerBound_.segment(discreteInd, constraintSize) = constraint->getLowerBoundsTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteLowerBound_;
+ }
+
+ virtual VectorXs getUpperBound() override
+ {
+ size_t discreteInd = 0;
+ size_t constraintSize = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteUpperBound_.segment(discreteInd, constraintSize) = constraint->getUpperBoundsIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteUpperBound_.segment(discreteInd, constraintSize) = constraint->getUpperBoundsTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteUpperBound_;
+ }
+
+ virtual size_t getConstraintSize() override { return constraintsCount_; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ size_t N_;
+
+ std::vector<std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> constraints_;
+
+ size_t constraintsCount_;
+ size_t constraintsIntermediateCount_;
+ size_t constraintsTerminalCount_;
+
+ VectorXs discreteConstraints_;
+ VectorXs discreteLowerBound_;
+ VectorXs discreteUpperBound_;
+
+ VectorXs discreteJac_;
+ Eigen::VectorXi discreteIRow_;
+ Eigen::VectorXi discreteJCol_;
+
+ size_t nonZeroJacCount_;
+ size_t nonZeroJacCountIntermediate_;
+ size_t nonZeroJacCountTerminal_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h
new file mode 100644
index 0000000..031f53e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h
@@ -0,0 +1,97 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+#include <ct/optcon/nlp/DiscreteConstraintContainerBase.h>
+#include <ct/optcon/dms/constraints/InitStateConstraint.h>
+#include <ct/optcon/dms/constraints/ContinuityConstraint.h>
+#include <ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+#include <ct/optcon/dms/constraints/ConstraintDiscretizer.h>
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Container class for the constraints used in DMS
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintsContainerDms : public tpl::DiscreteConstraintContainerBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The time grid
+ * @param[in] shotContainers The shot containers
+ * @param[in] constraintsIntermediate The intermediate constraints
+ * @param[in] constraintsFinal The final constraints
+ * @param[in] x0 The initial state
+ * @param[in] settings The dms settings
+ */
+ ConstraintsContainerDms(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers,
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints,
+ const state_vector_t& x0,
+ const DmsSettings settings);
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintsContainerDms(){};
+
+ virtual void prepareEvaluation() override;
+
+ virtual void prepareJacobianEvaluation() override;
+
+ /**
+ * @brief Updates the initial constraint
+ *
+ * @param[in] x0 The new initial state
+ */
+ void changeInitialConstraint(const state_vector_t& x0);
+
+private:
+ const DmsSettings settings_;
+
+ std::shared_ptr<InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_init_;
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+};
+
+#include "implementation/ConstraintsContainerDms-impl.h"
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h
new file mode 100644
index 0000000..0084915
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h
@@ -0,0 +1,351 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Implementation of the DMS continuity constraints
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ContinuityConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Default constructor
+ */
+ ContinuityConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] shotContainer The shot container
+ * @param[in] w The optimization variables
+ * @param[in] shotIndex The shot number
+ * @param[in] settings The dms settings
+ */
+ ContinuityConstraint(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>> shotContainer,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ size_t shotIndex,
+ const DmsSettings settings)
+ : shotContainer_(shotContainer), w_(w), shotIndex_(shotIndex), settings_(settings)
+ {
+ lb_.setConstant(SCALAR(0.0));
+ ub_.setConstant(SCALAR(0.0));
+
+ size_t nr = 0;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ nr = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM;
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ nr = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM + STATE_DIM * CONTROL_DIM;
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // nr += STATE_DIM;
+ jacLocal_.resize(nr);
+ }
+
+
+ virtual VectorXs eval() override
+ {
+ stateNext_ = shotContainer_->getStateIntegrated();
+ assert(stateNext_ == stateNext_);
+ assert(w_->getOptimizedState(shotIndex_ + 1) == w_->getOptimizedState(shotIndex_ + 1));
+ return w_->getOptimizedState(shotIndex_ + 1) - stateNext_;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ count_local_ = 0;
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ computeXblock(); // add the big block (derivative w.r.t. state s_i)
+ computeUblock(); // add the smaller block (derivative w.r.t. control q_i)
+ computeIblock(); // add the diagonal (derivative w.r.t. state s_(i+1))
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // computeHblock();
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ computeXblock(); // add the big block (derivative w.r.t. state s_i)
+ computeUblock(); // add the smaller block (derivative w.r.t. control q_i)
+ computeIblock(); // add the diagonal (derivative w.r.t. state s_(i+1))
+ computeUblock_2(); // add the smaller block (derivative w.r.t. control q_(i+1))
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // computeHblock();
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+ return jacLocal_;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override
+ {
+ size_t no = 0;
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ no = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM * 1;
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ no = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM * 1 + STATE_DIM * CONTROL_DIM;
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ /* the derivatives w.r.t. the time optimization variable (h_i) */
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // no += STATE_DIM;
+ // }
+
+ return no;
+ }
+
+
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t indexNumber = 0;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ // add the big block (derivative w.r.t. state)
+ indexNumber += BASE::genBlockIndices(
+ w_->getStateIndex(shotIndex_), STATE_DIM, STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the smaller block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the diagonal
+ indexNumber += BASE::genDiagonalIndices(
+ w_->getStateIndex(shotIndex_ + 1), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ // add the big block (derivative w.r.t. state)
+ indexNumber += BASE::genBlockIndices(
+ w_->getStateIndex(shotIndex_), STATE_DIM, STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the smaller block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the diagonal
+ indexNumber += BASE::genDiagonalIndices(
+ w_->getStateIndex(shotIndex_ + 1), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the fourth block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_ + 1), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ /* for the derivatives w.r.t. the time optimization variables (t_i) */
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // indexNumber += BASE::genBlockIndices(w_->getTimeSegmentIndex(shotIndex_),
+ // STATE_DIM, 1, iRow_vec, jCol_vec, indexNumber);
+ // }
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return STATE_DIM; }
+private:
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * states s_i
+ */
+ void computeXblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * inputs q_i
+ */
+ void computeUblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * inputs q_{i+1}
+ */
+ void computeUblock_2();
+
+ /**
+ * @brief Evaluates an identity matrix in sparse form
+ */
+ void computeIblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * time segments h_i
+ */
+ void computeHblock();
+
+ std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>> shotContainer_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ size_t shotIndex_;
+ const DmsSettings settings_;
+
+ VectorXs jacLocal_;
+ size_t count_local_;
+ state_vector_t stateNext_;
+
+ state_vector_t lb_;
+ state_vector_t ub_;
+};
+
+
+// template <size_t STATE_DIM, size_t CONTROL_DIM>
+// void ContinuityConstraint<STATE_DIM, CONTROL_DIM>::computeHblock()
+// {
+// // take last elements of state and time
+// state_vector_t state = shotContainer_->getStateIntegrated();
+// ct::core::Time time = shotContainer_->getIntegrationTimeFinal();
+
+// // compute derivative
+// state_vector_t mat;
+// state_vector_t dynamics;
+// shotContainer_->getControlledSystemPtr()->computeDynamics(state, time, dynamics);
+
+// switch (settings_.splineType_)
+// {
+// case DmsSettings::ZERO_ORDER_HOLD:
+// {
+// mat = - dynamics;
+// break;
+// }
+// case DmsSettings::PIECEWISE_LINEAR:
+// {
+// mat = -dynamics - shotContainer_->getdXdHiIntegrated();
+// break;
+// }
+// default:
+// {
+// throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+// }
+// }
+
+// jacLocal_.segment(count_local_, STATE_DIM) = mat;
+// count_local_ += STATE_DIM;
+// }
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeXblock()
+{
+ state_matrix_t mat = -shotContainer_->getdXdSiIntegrated();
+ mat.transposeInPlace();
+ VectorXs dXdSiVec = (Eigen::Map<VectorXs>(mat.data(), STATE_DIM * STATE_DIM));
+
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * STATE_DIM) = dXdSiVec;
+
+ count_local_ += STATE_DIM * STATE_DIM;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeUblock()
+{
+ MatrixXs mat = -shotContainer_->getdXdQiIntegrated();
+ mat.transposeInPlace();
+ VectorXs dXdQiVec = Eigen::Map<VectorXs>(mat.data(), STATE_DIM * CONTROL_DIM);
+
+ // // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * CONTROL_DIM) = dXdQiVec;
+ count_local_ += STATE_DIM * CONTROL_DIM;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeUblock_2()
+{
+ MatrixXs mat = -shotContainer_->getdXdQip1Integrated();
+ mat.transposeInPlace();
+ VectorXs dXdU1Vec = Eigen::Map<VectorXs>(mat.data(), STATE_DIM * CONTROL_DIM);
+
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * CONTROL_DIM) = dXdU1Vec;
+ count_local_ += STATE_DIM * CONTROL_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeIblock()
+{
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM) = VectorXs::Ones(STATE_DIM);
+ count_local_ += STATE_DIM;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h
new file mode 100644
index 0000000..fbe9821
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h
@@ -0,0 +1,79 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The implementation of the DMS initial state constraint
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class InitStateConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ InitStateConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] x0 The initial state
+ * @param[in] w The optimization variables
+ */
+ InitStateConstraint(const state_vector_t& x0, std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w)
+ : w_(w), x_0_(x0)
+ {
+ lb_.setConstant(SCALAR(0.0));
+ ub_.setConstant(SCALAR(0.0));
+ }
+
+ /**
+ * @brief Updates the constraint
+ *
+ * @param[in] x0 The new initial state
+ */
+ void updateConstraint(const state_vector_t& x0) { x_0_ = x0; }
+ virtual VectorXs eval() override { return w_->getOptimizedState(0) - x_0_; }
+ virtual VectorXs evalSparseJacobian() override { return state_vector_t::Ones(); }
+ virtual size_t getNumNonZerosJacobian() override { return (size_t)STATE_DIM; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t indexNumber = 0;
+ indexNumber += BASE::genDiagonalIndices(w_->getStateIndex(0), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return STATE_DIM; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ state_vector_t x_0_;
+
+ //Constraint bounds
+ state_vector_t lb_; // lower bound
+ state_vector_t ub_; // upper bound
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h
new file mode 100644
index 0000000..a968093
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h
@@ -0,0 +1,95 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This is the implementation of the time horizon constraint when
+ * using time grid optimization.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class TimeHorizonEqualityConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<double> BASE;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ TimeHorizonEqualityConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The dms time grid
+ * @param[in] settings The dms settings
+ */
+ TimeHorizonEqualityConstraint(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ DmsSettings settings)
+ : w_(w), timeGrid_(timeGrid), settings_(settings)
+ {
+ // lower bound is number of shot times the lower bound for each interval h
+ lb_ << SCALAR(settings_.N_ * settings_.h_min_ - settings_.T_);
+ ub_ << SCALAR(0.0);
+
+ std::cout << " ... time horizon lower bound: " << SCALAR(settings_.T_ + lb_(0)) << std::endl;
+ std::cout << " ... time horizon upper bound: " << SCALAR(settings_.T_ + ub_(0)) << std::endl;
+ }
+
+ virtual VectorXs eval() override
+ {
+ Eigen::Matrix<SCALAR, 1, 1> mat;
+ mat << SCALAR(timeGrid_->getOptimizedTimeHorizon() - settings_.T_);
+ return mat;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ VectorXs one(settings_.N_);
+ one.setConstant(SCALAR(1.0));
+ return one;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override { return settings_.N_; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ for (size_t i = 0; i < settings_.N_; ++i)
+ {
+ iRow_vec(i) = 0;
+ jCol_vec(i) = w_->getTimeSegmentIndex(i);
+ }
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return 1; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ DmsSettings settings_;
+
+ //Constraint bounds
+ Eigen::Matrix<SCALAR, 1, 1> lb_;
+ Eigen::Matrix<SCALAR, 1, 1> ub_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h b/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h
new file mode 100644
index 0000000..0238b43
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintsContainerDms(
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers,
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints,
+ const state_vector_t& x0,
+ const DmsSettings settings)
+ : settings_(settings), shotContainers_(shotContainers)
+{
+ c_init_ = std::shared_ptr<InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(x0, w));
+
+ this->constraints_.push_back(c_init_);
+
+ for (size_t shotNr = 0; shotNr < settings_.N_; shotNr++)
+ {
+ std::shared_ptr<ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_i =
+ std::shared_ptr<ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(shotContainers[shotNr], w, shotNr, settings));
+
+ this->constraints_.push_back(c_i);
+ }
+
+ if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ {
+ std::shared_ptr<TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_horizon_equal =
+ std::shared_ptr<TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(w, timeGrid, settings));
+
+ this->constraints_.push_back(c_horizon_equal);
+ }
+
+ if (discretizedConstraints)
+ {
+ std::cout << "Adding discretized constraints" << std::endl;
+ this->constraints_.push_back(discretizedConstraints);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::prepareEvaluation()
+{
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateShot();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::prepareJacobianEvaluation()
+{
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateSensitivities();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeInitialConstraint(const state_vector_t& x0)
+{
+ c_init_->updateConstraint(x0);
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms.h b/ct_optcon/include/ct/optcon/dms/dms.h
new file mode 100644
index 0000000..55f3e65
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms.h
@@ -0,0 +1,7 @@
+
+#pragma once
+
+// Include file for convenience
+
+#include "Constraints"
+#include "Dms"
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h b/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h
new file mode 100644
index 0000000..297e538
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h
@@ -0,0 +1,84 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \ingroup DMS
+ *
+ * \brief DMS controller class
+ *
+ * Implements a controller to be handed over to a system of type "ControlledSystem". This controller applies the nominal input trajectory designed by the algorithm,
+ * which is either piecewise constant or piecewise linear between the nodes.
+ *
+ * @tparam STATE_DIM: Dimension of the state vector
+ * @tparam INPUT_DIM: Dimension of the control input vector
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ControllerDms : public ct::core::Controller<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+
+ ControllerDms() = delete;
+
+ ControllerDms(std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner, size_t shotIdx)
+ : controlSpliner_(controlSpliner), shotIdx_(shotIdx)
+ {
+ }
+
+
+ ControllerDms(const ControllerDms& arg) : controlSpliner_(arg.controlSpliner_), shotIdx_(arg.shotIdx_) {}
+ virtual ~ControllerDms() {}
+ virtual ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+ }
+
+
+ void computeControl(const state_vector_t& state, const SCALAR& t, control_vector_t& controlAction)
+ {
+ controlAction = controlSpliner_->evalSpline(t, shotIdx_);
+ assert(controlAction == controlAction);
+ }
+
+ virtual core::ControlMatrix<CONTROL_DIM, SCALAR> getDerivativeU0(const state_vector_t& state,
+ const SCALAR time) override
+ {
+ return controlSpliner_->splineDerivative_q_i(time, shotIdx_);
+ }
+
+ virtual core::ControlMatrix<CONTROL_DIM, SCALAR> getDerivativeUf(const state_vector_t& state,
+ const SCALAR time) override
+ {
+ return controlSpliner_->splineDerivative_q_iplus1(time, shotIdx_);
+ }
+
+
+private:
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+
+ /* index of the shot to which this controller belongs */
+ size_t shotIdx_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h
new file mode 100644
index 0000000..380f591
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h
@@ -0,0 +1,46 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Defines basic types used in the DMS algorithm
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsDimensions
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> state_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> state_control_matrix_t;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> state_control_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef ct::core::ControlMatrixTrajectory<CONTROL_DIM, SCALAR> control_matrix_array_t;
+
+ typedef SCALAR time_t;
+ typedef ct::core::tpl::TimeArray<SCALAR> time_array_t;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h
new file mode 100644
index 0000000..a657a09
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h
@@ -0,0 +1,477 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+#include <ct/optcon/dms/constraints/ConstraintsContainerDms.h>
+#include <ct/optcon/dms/constraints/ConstraintDiscretizer.h>
+
+#include <ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h>
+#include <ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+#include <ct/optcon/nlp/Nlp.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class sets up the DMS problem
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsProblem : public tpl::Nlp<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ /**
+ * @brief Custom constructor, sets up the objects needed for the Dms
+ * algorithm depending on the settings
+ *
+ * @param[in] settings The Dms settings
+ * @param[in] systemPtrs The non linear systems
+ * @param[in] linearPtrs The linearized systems
+ * @param[in] costPtrs The cost function
+ * @param[in] boxConstraints The box constraints
+ * @param[in] generaConstraints The general constraints
+ * @param[in] x0 The initial state
+ */
+ DmsProblem(DmsSettings settings,
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systemPtrs,
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearPtrs,
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costPtrs,
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints,
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints,
+ const state_vector_t& x0)
+ : settings_(settings)
+ {
+ assert(systemPtrs.size() == settings_.N_);
+ assert(linearPtrs.size() == settings_.N_);
+ assert(costPtrs.size() == settings_.N_);
+ settings_.parametersOk();
+
+ timeGrid_ = std::shared_ptr<tpl::TimeGrid<SCALAR>>(new tpl::TimeGrid<SCALAR>(settings.N_, settings.T_));
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ controlSpliner_ = std::shared_ptr<ZeroOrderHoldSpliner<control_vector_t, SCALAR>>(
+ new ZeroOrderHoldSpliner<control_vector_t, SCALAR>(timeGrid_));
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ controlSpliner_ = std::shared_ptr<LinearSpliner<control_vector_t, SCALAR>>(
+ new LinearSpliner<control_vector_t, SCALAR>(timeGrid_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown spline type"));
+ }
+
+
+ size_t wLength = (settings.N_ + 1) * (STATE_DIM + CONTROL_DIM);
+ if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ {
+ throw std::runtime_error("Currently we do not support adaptive time gridding");
+ // wLength += settings_.N_;
+ }
+
+ this->optVariables_ = std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>(wLength, settings));
+
+ optVariablesDms_ = std::static_pointer_cast<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>>(this->optVariables_);
+
+ if (boxConstraints.size() > 0 || generalConstraints.size() > 0)
+ discretizedConstraints_ = std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>(
+ optVariablesDms_, controlSpliner_, timeGrid_, settings_.N_));
+
+ if (boxConstraints.size() > 0)
+ discretizedConstraints_->setBoxConstraints(boxConstraints.front());
+
+ if (generalConstraints.size() > 0)
+ discretizedConstraints_->setGeneralConstraints(generalConstraints.front());
+
+
+ for (size_t shotIdx = 0; shotIdx < settings_.N_; shotIdx++)
+ {
+ std::shared_ptr<ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>> newController(
+ new ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>(controlSpliner_, shotIdx));
+ systemPtrs[shotIdx]->setController(newController);
+ linearPtrs[shotIdx]->setController(newController);
+
+ size_t nIntegrationSteps =
+ (timeGrid_->getShotEndTime(shotIdx) - timeGrid_->getShotStartTime(shotIdx)) / settings_.dt_sim_ + 0.5;
+
+ shotContainers_.push_back(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>(systemPtrs[shotIdx], linearPtrs[shotIdx],
+ costPtrs[shotIdx], optVariablesDms_, controlSpliner_, timeGrid_, shotIdx, settings_,
+ nIntegrationSteps)));
+ }
+
+ switch (settings_.costEvaluationType_)
+ {
+ case DmsSettings::SIMPLE:
+ {
+ this->costEvaluator_ = std::shared_ptr<CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>(
+ costPtrs.front(), optVariablesDms_, timeGrid_, settings_));
+ break;
+ }
+ case DmsSettings::FULL:
+ {
+ this->costEvaluator_ = std::shared_ptr<CostEvaluatorFull<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new CostEvaluatorFull<STATE_DIM, CONTROL_DIM, SCALAR>(
+ costPtrs.front(), optVariablesDms_, controlSpliner_, shotContainers_, settings_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown cost evaluation type"));
+ }
+
+ this->constraints_ = std::shared_ptr<ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>(
+ optVariablesDms_, timeGrid_, shotContainers_, discretizedConstraints_, x0, settings_));
+
+ this->optVariables_->resizeConstraintVars(this->getConstraintsCount());
+ }
+
+ void generateAndCompileCode(
+ std::vector<std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>> systemPtrs,
+ std::vector<std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>> linearPtrs,
+ std::vector<std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ costPtrs,
+ std::vector<std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ boxConstraints,
+ std::vector<std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ generalConstraints,
+ const ct::core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x0)
+ {
+ typedef ct::core::ADCGScalar ScalarCG;
+ size_t wLength = (settings_.N_ + 1) * (STATE_DIM + CONTROL_DIM);
+
+ std::shared_ptr<tpl::TimeGrid<ScalarCG>> timeGrid(
+ new tpl::TimeGrid<ScalarCG>(settings_.N_, ScalarCG(settings_.T_)));
+
+ std::shared_ptr<SplinerBase<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>> controlSpliner;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ controlSpliner =
+ std::shared_ptr<ZeroOrderHoldSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>>(
+ new ZeroOrderHoldSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>(timeGrid));
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ controlSpliner =
+ std::shared_ptr<LinearSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>>(
+ new LinearSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>(timeGrid));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown spline type"));
+ }
+
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, ScalarCG>> optVariablesDms(
+ new OptVectorDms<STATE_DIM, CONTROL_DIM, ScalarCG>(wLength, settings_));
+
+
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>> discretizedConstraints;
+ if (boxConstraints.size() > 0 || generalConstraints.size() > 0)
+ discretizedConstraints = std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ optVariablesDms, controlSpliner, timeGrid, settings_.N_));
+
+ if (boxConstraints.size() > 0)
+ discretizedConstraints->setBoxConstraints(boxConstraints.front());
+
+ if (generalConstraints.size() > 0)
+ discretizedConstraints->setGeneralConstraints(generalConstraints.front());
+
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>>> shotContainers;
+ for (size_t shotIdx = 0; shotIdx < settings_.N_; shotIdx++)
+ {
+ std::shared_ptr<ControllerDms<STATE_DIM, CONTROL_DIM, ScalarCG>> newController(
+ new ControllerDms<STATE_DIM, CONTROL_DIM, ScalarCG>(controlSpliner, shotIdx));
+ systemPtrs[shotIdx]->setController(newController);
+ linearPtrs[shotIdx]->setController(newController);
+
+ size_t nIntegrationSteps =
+ (timeGrid_->getShotEndTime(shotIdx) - timeGrid_->getShotStartTime(shotIdx)) / settings_.dt_sim_ + 0.5;
+
+ shotContainers.push_back(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>(systemPtrs[shotIdx], linearPtrs[shotIdx],
+ costPtrs[shotIdx], optVariablesDms, controlSpliner, timeGrid, shotIdx, settings_,
+ nIntegrationSteps)));
+ }
+
+ std::shared_ptr<tpl::DiscreteCostEvaluatorBase<ScalarCG>> costEvaluator;
+
+ switch (settings_.costEvaluationType_)
+ {
+ case DmsSettings::SIMPLE:
+ {
+ costEvaluator = std::shared_ptr<CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ costPtrs.front(), optVariablesDms, timeGrid, settings_));
+ break;
+ }
+ case DmsSettings::FULL:
+ {
+ costEvaluator = std::shared_ptr<CostEvaluatorFull<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new CostEvaluatorFull<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ costPtrs.front(), optVariablesDms, controlSpliner, shotContainers, settings_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown cost evaluation type"));
+ }
+
+ optVariablesDms->resizeConstraintVars(this->getConstraintsCount());
+
+ std::shared_ptr<ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, ScalarCG>> constraints(
+ new ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ optVariablesDms, timeGrid, shotContainers, discretizedConstraints, x0, settings_));
+
+ if (settings_.solverSettings_.useGeneratedCostGradient_)
+ {
+ std::function<Eigen::Matrix<ScalarCG, 1, 1>(const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>&)> fCost = [&](
+ const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>& xOpt) {
+ optVariablesDms->setOptimizationVars(xOpt);
+
+ controlSpliner->computeSpline(optVariablesDms->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers)
+ shotContainer->reset();
+
+ Eigen::Matrix<ScalarCG, 1, 1> out;
+ out << costEvaluator->eval();
+ return out;
+ };
+
+ settings_.cppadSettings_.createJacobian_ = true;
+
+ this->costCodegen_ = std::shared_ptr<ct::core::DerivativesCppadJIT<-1, 1>>(
+ new ct::core::DerivativesCppadJIT<-1, 1>(fCost, this->getVarCount()));
+ this->costCodegen_->compileJIT(settings_.cppadSettings_, "dmsCostFunction");
+ }
+
+ if (settings_.solverSettings_.useGeneratedConstraintJacobian_)
+ {
+ std::function<Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>(const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>&)>
+ fConstraints = [&](const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>& xOpt) {
+ optVariablesDms->setOptimizationVars(xOpt);
+
+ controlSpliner->computeSpline(optVariablesDms->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers)
+ shotContainer->reset();
+
+
+ Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1> out(
+ this->getConstraintsCount()); // out.resize(this->getConstraintsCount, 1);
+ constraints->evalConstraints(out);
+ return out;
+ };
+
+ settings_.cppadSettings_.createJacobian_ = false;
+
+ this->constraintsCodegen_ =
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, -1>>(new ct::core::DerivativesCppadJIT<-1, -1>(
+ fConstraints, this->getVarCount(), this->getConstraintsCount()));
+ this->constraintsCodegen_->compileJIT(settings_.cppadSettings_, "dmsConstraints");
+ }
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~DmsProblem() {}
+ virtual void updateProblem() override
+ {
+ controlSpliner_->computeSpline(optVariablesDms_->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers_)
+ shotContainer->reset();
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // // std::cout << "optVariablesDms_->getOptimizedTimeSegments(): " << optVariablesDms_->getOptimizedTimeSegments().transpose() << std::endl;
+ // timeGrid_->updateTimeGrid(optVariablesDms_->getOptimizedTimeSegments());
+ // }
+ }
+
+ /**
+ * @brief Updates the settings
+ *
+ * @param[in] settings New dms settings
+ */
+ void configure(const DmsSettings& settings) { settings_ = settings; }
+ /**
+ * @brief Retrieves the solution state trajectory at every shot
+ *
+ * @return The state solution trajectory
+ */
+ const state_vector_array_t& getStateSolution() { return optVariablesDms_->getOptimizedStates(); }
+ /**
+ * @brief Retrieves the solution control trajectory at every shot
+ *
+ * @return The control solution trajectory
+ */
+ const control_vector_array_t& getInputSolution() { return optVariablesDms_->getOptimizedInputs(); }
+ /**
+ * @brief Retrieves the solution time trajectory at every shot
+ *
+ * @return The time solution trajectory
+ */
+ const time_array_t& getTimeSolution() { return timeGrid_->toImplementation(); }
+ /**
+ * @brief Retrieves a dense state solution trajectory
+ *
+ * @return The dense state solution trajectory
+ */
+ const state_vector_array_t& getStateTrajectory()
+ {
+ // stateSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // stateSolutionDense_.push_back(shotContainers_.front()->getXHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // state_vector_array_t x_traj = shotContainer->getXHistory();
+ // for(size_t j = 1; j < x_traj.size(); ++j)
+ // stateSolutionDense_.push_back(x_traj[j]);
+ // }
+ return optVariablesDms_->getOptimizedStates();
+ ;
+ }
+
+ /**
+ * @brief Retrieves a dense input solution trajectory
+ *
+ * @return The dense control solution trajectory
+ */
+ const control_vector_array_t& getInputTrajectory()
+ {
+ // inputSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // inputSolutionDense_.push_back(shotContainers_.front()->getUHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // control_vector_array_t u_traj = shotContainer->getUHistory();
+ // for(size_t j = 1; j < u_traj.size(); ++j)
+ // inputSolutionDense_.push_back(u_traj[j]);
+ // }
+ // return inputSolutionDense_;
+ return optVariablesDms_->getOptimizedInputs();
+ }
+
+ /**
+ * @brief Retrieves a dense time solution trajectory
+ *
+ * @return The dense time solution trajectory
+ */
+ const time_array_t& getTimeArray()
+ {
+ // timeSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // timeSolutionDense_.push_back(shotContainers_.front()->getTHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // time_array_t t_traj = shotContainer->getTHistory();
+ // for(size_t j = 1; j < t_traj.size(); ++j)
+ // timeSolutionDense_.push_back(t_traj[j]);
+ // }
+ // return timeSolutionDense_;
+ return timeGrid_->toImplementation();
+ }
+
+ /**
+ * @brief Sets the initial guess of the optimization
+ *
+ * @param[in] x_init_guess The state trajectory initial guess
+ * @param[in] u_init_guess The control trajectory initial guess
+ * @param[in] t_init_guess The time trajectory initial guess
+ */
+ void setInitialGuess(const state_vector_array_t& x_init_guess,
+ const control_vector_array_t& u_init_guess,
+ const time_array_t& t_init_guess = time_array_t(0.0))
+ {
+ optVariablesDms_->setInitGuess(x_init_guess, u_init_guess);
+ }
+
+ /**
+ * @brief Return the timehorizon of the problem
+ *
+ * @return The time horizon
+ */
+ const core::Time getTimeHorizon() const { return timeGrid_->getTimeHorizon(); }
+ /**
+ * @brief Updates the timehorizon
+ *
+ * @param[in] tf The new time horizon
+ */
+ void changeTimeHorizon(const SCALAR tf) { timeGrid_->changeTimeHorizon(tf); }
+ /**
+ * @brief Updates the initial state
+ *
+ * @param[in] x0 The new inital state
+ */
+ void changeInitialState(const state_vector_t& x0)
+ {
+ // constraints_->changeInitialConstraint(x0);
+ optVariablesDms_->changeInitialState(x0);
+ }
+
+ /**
+ * @brief Prints the solution trajectories
+ */
+ void printSolution() { optVariablesDms_->printoutSolution(); }
+private:
+ DmsSettings settings_;
+
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints_;
+
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> optVariablesDms_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+
+ state_vector_array_t stateSolutionDense_;
+ control_vector_array_t inputSolutionDense_;
+ time_array_t timeSolutionDense_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h
new file mode 100644
index 0000000..701bfe4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h
@@ -0,0 +1,166 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <map>
+
+#include <boost/property_tree/info_parser.hpp>
+
+#include <ct/optcon/nlp/solver/NlpSolverSettings.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Defines the DMS settings
+ */
+class DmsSettings
+{
+public:
+ typedef enum SplineType { ZERO_ORDER_HOLD = 0, PIECEWISE_LINEAR = 1, num_types_splining } SplineType_t;
+ typedef enum ObjectiveType { KEEP_TIME_AND_GRID = 0, OPTIMIZE_GRID = 1, num_types_objectives } ObjectiveType_t;
+ typedef enum IntegrationType { EULER = 0, RK4 = 1, RK5 = 2, num_types_integration } IntegrationType_t;
+ typedef enum CostEvaluationType { SIMPLE = 0, FULL = 1, num_types_costevaluation } CostEvaluationType_t;
+
+ /**
+ * @brief Default constructor. Sets some default DMS settings. Note
+ * that the optimal settings are strongly dependent on the
+ * problem and it is highly recommended to use custom settings
+ */
+ DmsSettings()
+ : N_(30),
+ T_(5),
+ nThreads_(1),
+ splineType_(ZERO_ORDER_HOLD),
+ costEvaluationType_(SIMPLE),
+ objectiveType_(KEEP_TIME_AND_GRID),
+ h_min_(0.1),
+ integrationType_(RK4),
+ dt_sim_(0.01),
+ absErrTol_(1e-10),
+ relErrTol_(1e-10)
+ {
+ }
+
+ size_t N_; // the number of shots
+ double T_; // the time horizon
+ size_t nThreads_; // number of threads
+ SplineType_t splineType_; // spline interpolation type between the nodes
+ CostEvaluationType_t costEvaluationType_; // the the of costevaluator
+ ObjectiveType_t objectiveType_; // Timegrid optimization on(expensive) or off?
+ double h_min_; // minimum admissible distance between two nodes in [sec]
+ IntegrationType_t integrationType_; // the integration type between the nodes
+ double dt_sim_; // and the corresponding integration timestep
+ double absErrTol_; // the absolute and relative integrator tolerances when using RK5
+ double relErrTol_;
+
+ NlpSolverSettings solverSettings_;
+ ct::core::DerivativesCppadSettings cppadSettings_;
+
+ void print()
+ {
+ std::cout << "=============================================================" << std::endl;
+ std::cout << "\tMultiple Shooting Settings: " << std::endl;
+ std::cout << "=============================================================" << std::endl;
+
+ std::cout << "Shooting intervals N : " << N_ << std::endl;
+ std::cout << "Total Time horizon: " << T_ << "s" << std::endl;
+ std::cout << "Number of threads: " << nThreads_ << std::endl;
+ std::cout << "Splinetype: " << splineToString[splineType_] << std::endl;
+ std::cout << "Cost eval: " << costEvalToString[costEvaluationType_] << std::endl;
+ std::cout << "Objective type: " << objTypeToString[objectiveType_] << std::endl;
+ std::cout << "Integration type: " << integratorToString[integrationType_] << std::endl;
+ std::cout << "Simulation timestep dt_sim: " << dt_sim_ << std::endl;
+
+ std::cout << "=============================================================" << std::endl;
+
+ solverSettings_.print();
+ }
+
+ bool parametersOk() const
+ {
+ if (N_ < 1 || N_ > 1000)
+ return false;
+
+ if (T_ <= 0.0)
+ return false;
+
+ if (nThreads_ < 1 || nThreads_ > 32)
+ return false;
+
+ if (splineType_ < 0 || !(splineType_ < SplineType_t::num_types_splining))
+ return false;
+
+ if (costEvaluationType_ < 0 || !(costEvaluationType_ < CostEvaluationType_t::num_types_costevaluation))
+ return false;
+
+ if (objectiveType_ < 0 || !(objectiveType_ < ObjectiveType_t::num_types_objectives))
+ return false;
+
+ if (h_min_ < 0.01 || h_min_ > T_)
+ return false;
+
+ if (integrationType_ < 0 || !(integrationType_ < IntegrationType_t::num_types_integration))
+ return false;
+
+ if (dt_sim_ <= 0.0 || dt_sim_ > 100.0)
+ return false;
+
+ if (absErrTol_ <= 1e-20 || absErrTol_ > 1.0)
+ return false;
+
+ if (relErrTol_ <= 1e-20 || relErrTol_ > 1.0)
+ return false;
+
+ return solverSettings_.parametersOk();
+ }
+
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms")
+ {
+ if (verbose)
+ std::cout << "Trying to load DMS config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ N_ = pt.get<unsigned int>(ns + ".N");
+ T_ = pt.get<double>(ns + ".T");
+ nThreads_ = pt.get<unsigned int>(ns + ".nThreads");
+ splineType_ = (SplineType_t)pt.get<unsigned int>(ns + ".InterpolationType");
+ costEvaluationType_ = (CostEvaluationType_t)pt.get<unsigned int>(ns + ".CostEvaluationType");
+ objectiveType_ = (ObjectiveType_t)pt.get<unsigned int>(ns + ".ObjectiveType");
+ h_min_ = pt.get<double>(ns + ".h_min");
+
+ integrationType_ = (IntegrationType_t)pt.get<unsigned int>(ns + ".IntegrationType");
+ dt_sim_ = pt.get<double>(ns + ".dt_sim");
+ absErrTol_ = pt.get<double>(ns + ".AbsErrTol");
+ relErrTol_ = pt.get<double>(ns + ".RelErrTol");
+
+ solverSettings_.load(filename, verbose, ns + ".solver"); // todo bring in again
+ cppadSettings_.load(filename, verbose, ns + ".cppad");
+
+ if (verbose)
+ {
+ std::cout << "Loaded DMS config from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+private:
+ std::map<SplineType, std::string> splineToString = {
+ {ZERO_ORDER_HOLD, "Zero order hold"}, {PIECEWISE_LINEAR, "Piecewise Linear"}};
+ std::map<ObjectiveType, std::string> objTypeToString = {
+ {KEEP_TIME_AND_GRID, "Timegrid fix"}, {OPTIMIZE_GRID, "Timegrid Optimization On"}};
+ std::map<IntegrationType, std::string> integratorToString = {
+ {EULER, "Euler"}, {RK4, "Runge-Kutta 4th order"}, {RK5, "RK5 adaptive step size"}};
+ std::map<CostEvaluationType, std::string> costEvalToString = {{SIMPLE, "Simple"}, {FULL, "Full"}};
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h
new file mode 100644
index 0000000..70160ae
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h
@@ -0,0 +1,314 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include <ct/optcon/dms/dms_core/DmsProblem.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+#include <ct/optcon/nlp/Nlp>
+
+#include <memory>
+
+namespace ct {
+namespace optcon {
+
+/** @defgroup DMS DMS
+ *
+ * @brief The direct multiple shooting module
+ */
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The DMS policy used as a solution container
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+struct DmsPolicy
+{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ state_vector_array_t xSolution_;
+ control_vector_array_t uSolution_;
+ time_array_t tSolution_;
+};
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Class to solve a specfic DMS problem
+ *
+ * An example employing different DMS solvers is given in unit test \ref oscDMSTest.cpp
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsSolver : public OptConSolver<DmsSolver<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsSettings,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef OptConSolver<DmsSolver<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsSettings,
+ STATE_DIM,
+ CONTROL_DIM>
+ Base;
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+ /**
+ * @brief Custom constructor, converts the optcon problem to a DMS problem
+ *
+ * @param[in] problem The optimal control problem
+ * @param[in] settingsDms The dms settings
+ */
+ DmsSolver(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> problem, DmsSettings settingsDms)
+ : nlpSolver_(nullptr), settings_(settingsDms)
+ {
+ // Create system, linearsystem and costfunction instances
+ this->setProblem(problem);
+
+ dmsProblem_ = std::shared_ptr<DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>(settingsDms, this->systems_, this->linearSystems_,
+ this->costFunctions_, this->boxConstraints_, this->generalConstraints_, x0_));
+
+ // SNOPT only works for the double type
+ if (settingsDms.solverSettings_.solverType_ == NlpSolverSettings::SNOPT)
+ nlpSolver_ = std::shared_ptr<SnoptSolver>(new SnoptSolver(dmsProblem_, settingsDms.solverSettings_));
+ else if (settingsDms.solverSettings_.solverType_ == NlpSolverSettings::IPOPT)
+ nlpSolver_ = std::shared_ptr<IpoptSolver>(new IpoptSolver(dmsProblem_, settingsDms.solverSettings_));
+ else
+ std::cout << "Unknown solver type... Exiting" << std::endl;
+
+ configure(settingsDms);
+ }
+
+ virtual void generateAndCompileCode(const OptConProblem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>& problemCG,
+ const ct::core::DerivativesCppadSettings& settings) override
+ {
+ // Create system, linearsystem and costfunction instances
+ typedef std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> SysPtrCG;
+ typedef std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> LinearSysPtrCG;
+ typedef std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> CostPtrCG;
+ typedef std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>
+ ConstraintCG;
+ ct::core::StateVector<STATE_DIM, ct::core::ADCGScalar> x0CG = problemCG.getInitialState();
+
+ std::vector<SysPtrCG> systemsCG;
+ std::vector<LinearSysPtrCG> linearSystemsCG;
+ std::vector<CostPtrCG> costFunctionsCG;
+ std::vector<ConstraintCG> boxConstraintsCG;
+ std::vector<ConstraintCG> generalConstraintsCG;
+
+ for (size_t i = 0; i < settings_.N_; i++)
+ {
+ systemsCG.push_back(SysPtrCG(problemCG.getNonlinearSystem()->clone()));
+ linearSystemsCG.push_back(LinearSysPtrCG(problemCG.getLinearSystem()->clone()));
+ costFunctionsCG.push_back(CostPtrCG(problemCG.getCostFunction()->clone()));
+ }
+
+ if (problemCG.getBoxConstraints())
+ boxConstraintsCG.push_back(ConstraintCG(problemCG.getBoxConstraints()->clone()));
+
+ if (problemCG.getGeneralConstraints())
+ generalConstraintsCG.push_back(ConstraintCG(problemCG.getGeneralConstraints()->clone()));
+
+ dmsProblem_->generateAndCompileCode(
+ systemsCG, linearSystemsCG, costFunctionsCG, boxConstraintsCG, generalConstraintsCG, x0CG);
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~DmsSolver() {}
+ virtual void configure(const DmsSettings& settings) override
+ {
+ dmsProblem_->configure(settings);
+ dmsProblem_->changeTimeHorizon(tf_);
+ dmsProblem_->changeInitialState(x0_);
+ }
+
+ virtual bool solve() override
+ {
+ if (!nlpSolver_->isInitialized())
+ nlpSolver_->configure(settings_.solverSettings_);
+
+ return nlpSolver_->solve();
+ }
+
+ virtual const Policy_t& getSolution() override
+ {
+ policy_.xSolution_ = dmsProblem_->getStateSolution();
+ policy_.uSolution_ = dmsProblem_->getInputSolution();
+ policy_.tSolution_ = dmsProblem_->getTimeSolution();
+ return policy_;
+ }
+
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const override
+ {
+ return core::StateTrajectory<STATE_DIM, SCALAR>(dmsProblem_->getTimeArray(), dmsProblem_->getStateTrajectory());
+ }
+
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const override
+ {
+ return core::ControlTrajectory<CONTROL_DIM, SCALAR>(
+ dmsProblem_->getTimeArray(), dmsProblem_->getInputTrajectory());
+ }
+
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const override { return dmsProblem_->getTimeArray(); }
+ virtual void setInitialGuess(const Policy_t& initialGuess) override
+ {
+ dmsProblem_->setInitialGuess(initialGuess.xSolution_, initialGuess.uSolution_);
+ }
+
+ virtual SCALAR getTimeHorizon() const override { return dmsProblem_->getTimeHorizon(); }
+ virtual void changeTimeHorizon(const SCALAR& tf) override
+ {
+ tf_ = tf;
+ if (dmsProblem_)
+ dmsProblem_->changeTimeHorizon(tf);
+ }
+
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) override
+ {
+ x0_ = x0;
+ if (dmsProblem_)
+ dmsProblem_->changeInitialState(x0);
+ }
+
+ virtual void changeCostFunction(const typename Base::OptConProblem_t::CostFunctionPtr_t& cf) override
+ {
+ this->getCostFunctionInstances().resize(settings_.N_);
+ if (cf)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getCostFunctionInstances()[i] = typename Base::OptConProblem_t::CostFunctionPtr_t(cf->clone());
+ }
+
+ virtual void changeNonlinearSystem(const typename Base::OptConProblem_t::DynamicsPtr_t& dyn) override
+ {
+ this->getNonlinearSystemsInstances().resize(settings_.N_);
+
+ if (dyn)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getNonlinearSystemsInstances()[i] = typename Base::OptConProblem_t::DynamicsPtr_t(dyn->clone());
+ }
+
+ virtual void changeLinearSystem(const typename Base::OptConProblem_t::LinearPtr_t& lin) override
+ {
+ this->getLinearSystemsInstances().resize(settings_.N_);
+
+ if (lin)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getLinearSystemsInstances()[i] = typename Base::OptConProblem_t::LinearPtr_t(lin->clone());
+ }
+
+ virtual void changeBoxConstraints(const typename Base::OptConProblem_t::ConstraintPtr_t con) override
+ {
+ this->getBoxConstraintsInstances().push_back(typename Base::OptConProblem_t::ConstraintPtr_t(con->clone()));
+ }
+
+ virtual void changeGeneralConstraints(const typename Base::OptConProblem_t::ConstraintPtr_t con) override
+ {
+ this->getGeneralConstraintsInstances().push_back(typename Base::OptConProblem_t::ConstraintPtr_t(con->clone()));
+ }
+
+ /**
+ * @brief Prints out the solution trajectories of the DMS problem
+ */
+ void printSolution() { dmsProblem_->printSolution(); }
+ virtual std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() override
+ {
+ return systems_;
+ }
+ virtual const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const override
+ {
+ return systems_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() override
+ {
+ return linearSystems_;
+ }
+ virtual const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const override
+ {
+ return linearSystems_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() override
+ {
+ return costFunctions_;
+ }
+ virtual const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const override
+ {
+ return costFunctions_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() override
+ {
+ return boxConstraints_;
+ }
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const override
+ {
+ return boxConstraints_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() override
+ {
+ return generalConstraints_;
+ }
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances()
+ const override
+ {
+ return generalConstraints_;
+ }
+
+
+private:
+ std::shared_ptr<DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>> dmsProblem_; /*!<The dms problem*/
+ std::shared_ptr<tpl::NlpSolver<SCALAR>> nlpSolver_; /*!<The nlp solver for solving the dmsproblem*/
+ DmsSettings settings_; /*!<The dms settings*/
+
+ Policy_t policy_; /*!<The solution container*/
+
+ state_vector_t x0_; /*!<The initial state for the optimization*/
+ SCALAR tf_; /*!<The timehorizon of the problem*/
+
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systems_;
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearSystems_;
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costFunctions_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h b/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h
new file mode 100644
index 0000000..3ec4aa7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h
@@ -0,0 +1,214 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+#include <Eigen/Core>
+#include <map>
+
+#include "TimeGrid.h"
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+#include <ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h>
+#include <ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h>
+#include <ct/optcon/nlp/OptVector.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class is a wrapper around the NLP Optvector. It wraps the
+ * Vectors from the NLP solvers into state, control and time
+ * trajectories
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class OptVectorDms : public tpl::OptVector<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef tpl::OptVector<SCALAR> Base;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] n The number of optimization variables
+ * @param[in] settings The dms settings
+ */
+ OptVectorDms(size_t n, const DmsSettings& settings);
+
+ OptVectorDms(const OptVectorDms& arg) = delete;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~OptVectorDms() {}
+ /**
+ * @brief Returns the optimized state for a specific shot
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The optimized state
+ */
+ state_vector_t getOptimizedState(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the optimized control input for a specific shot
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The optimized control input
+ */
+ control_vector_t getOptimizedControl(const size_t pairNum) const;
+
+ /**
+ * @brief Return the optimized time segment for a specific shot
+ *
+ * @param[in] pairNum The pair number
+ *
+ * @return The optimized time segment.
+ */
+ SCALAR getOptimizedTimeSegment(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the optimized state for all shots
+ *
+ * @return The optimized states.
+ */
+ const state_vector_array_t& getOptimizedStates();
+
+ /**
+ * @brief Returns the optimized control inputs for all shots
+ *
+ * @return The optimized control inputs.
+ */
+ const control_vector_array_t& getOptimizedInputs();
+
+ /**
+ * @brief Returns the optimized time segments for all shots
+ *
+ * @return The optimized time segments.
+ */
+ const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& getOptimizedTimeSegments();
+
+ /**
+ * @brief Returns the starting index for the state at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getStateIndex(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the starting index for the control input at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getControlIndex(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the starting index for the time segment input at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getTimeSegmentIndex(const size_t pairNum) const;
+
+
+ /**
+ * @brief Sets an initial guess for the optimal solution. The optimal
+ * solution is set as a linear interpolation between inital
+ * state x0 and final state xf. The initial guess for the
+ * control input is assumed to be constant and equal to u0
+ *
+ * @param[in] x0 The initial state
+ * @param[in] x_f The final state
+ * @param[in] u0 The control input
+ */
+ void setInitGuess(const state_vector_t& x0, const state_vector_t& x_f, const control_vector_t& u0);
+
+ /**
+ * @brief Sets an initial guess for the optimal solution.
+ *
+ * @param[in] x_init Initial guess for the state trajectory
+ * @param[in] u_init Initial guess for the control trajectory
+ */
+ void setInitGuess(const state_vector_array_t& x_init, const control_vector_array_t& u_init);
+
+ /**
+ * @brief Updates the initial state
+ *
+ * @param[in] x0 The new initial state
+ */
+ void changeInitialState(const state_vector_t& x0);
+
+ /**
+ * @brief Updates the final state
+ *
+ * @param[in] xF The new final state
+ */
+ void changeDesiredState(const state_vector_t& xF);
+
+ /**
+ * @brief Sets bounds on the time segment variables
+ */
+ void setLowerTimeSegmentBounds();
+
+ /**
+ * @brief Returns the number of pairs
+ *
+ * @return Number of pairs
+ */
+ size_t numPairs() { return numPairs_; }
+ /**
+ * @brief Prints out the solution trajectories
+ */
+ void printoutSolution();
+
+private:
+ DmsSettings settings_;
+
+ size_t numPairs_;
+ /* maps the number of a "pair" to the index in w where ... */
+ std::map<size_t, size_t> pairNumToStateIdx_; /* ... its state starts */
+ std::map<size_t, size_t> pairNumToControlIdx_; /* ... its control starts */
+ std::map<size_t, size_t>
+ shotNumToShotDurationIdx_; /* its shot time starts is in w (last element doesn't have one) */
+
+ state_vector_array_t stateSolution_;
+ control_vector_array_t inputSolution_;
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> optimizedTimeSegments_;
+};
+
+} // namespace optcon
+} // namespace ct
+
+#include "implementation/OptVectorDms-impl.h"
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h b/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h
new file mode 100644
index 0000000..f74afa2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h
@@ -0,0 +1,426 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class implements analytical sensitivity generation for the
+ * euler and rk4 integration scheme
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class RKnDerivatives
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ RKnDerivatives() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] controlSpliner The control spliner
+ * @param[in] shotIdx The shot number
+ * @param[in] settings The dms settings
+ */
+ RKnDerivatives(std::shared_ptr<SplinerBase<control_vector_t>> controlSpliner,
+ size_t shotIdx,
+ const DmsSettings& settings)
+ : shotIdx_(shotIdx),
+ settings_(settings),
+ controlSpliner_(controlSpliner),
+ x_log_(nullptr),
+ u_log_(nullptr),
+ t_log_(nullptr),
+ A_log_(nullptr),
+ B_log_(nullptr),
+ dXdSi_history_(state_matrix_array_t(0)),
+ dXdQi_history_(state_control_matrix_array_t(0)),
+ dXdQip1_history_(state_control_matrix_array_t(0)),
+ dXdHi_history_(state_vector_array_t(0))
+ {
+ }
+
+ void setLogs(std::shared_ptr<state_vector_array_t> x_log,
+ std::shared_ptr<control_vector_array_t> u_log,
+ std::shared_ptr<time_array_t> t_log,
+ std::shared_ptr<state_matrix_array_t> A_log,
+ std::shared_ptr<state_control_matrix_array_t> B_log)
+ {
+ x_log_ = x_log;
+ u_log_ = u_log;
+ t_log_ = t_log;
+ A_log_ = A_log;
+ B_log_ = B_log;
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. s_i
+ void compute_dXdSi()
+ {
+ dXdSi_history_.clear();
+ dXdSi_history_.push_back(Eigen::MatrixXd::Identity(STATE_DIM, STATE_DIM));
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ size_t nSteps = A_log_->size() / 1;
+ for (size_t i = 0; i < nSteps; i++)
+ {
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ state_matrix_t temp = Eigen::MatrixXd::Identity(STATE_DIM, STATE_DIM) + dt_sim * (*A_log_)[i];
+
+ dXdSi_history_.push_back(temp * dXdSi_history_.back());
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1.0;
+
+ double b1 = 1.0 / 6.0;
+ double b2 = 1.0 / 3.0;
+ double b3 = 1.0 / 3.0;
+ double b4 = 1.0 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // step 1:
+ state_matrix_t dK1dSi = (*A_log_)[i * m] * dXdSi_history_.back();
+
+ // step 2:
+ state_matrix_t dK2dSi = (*A_log_)[i * m + 1] * (dXdSi_history_.back() + h * a_21 * dK1dSi);
+
+ // step 3:
+ state_matrix_t dK3dSi = (*A_log_)[i * m + 2] * (dXdSi_history_.back() + h * a_32 * dK2dSi);
+
+ // step 4:
+ state_matrix_t dK4dSi = (*A_log_)[i * m + 3] * (dXdSi_history_.back() + h * a_43 * dK3dSi);
+
+
+ // summation:
+ state_matrix_t temp =
+ dXdSi_history_.back() + h * (b1 * dK1dSi + b2 * dK2dSi + b3 * dK3dSi + b4 * dK4dSi);
+
+ dXdSi_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. q_i
+ void compute_dXdQi()
+ {
+ dXdQi_history_.clear();
+ dXdQi_history_.push_back(state_control_matrix_t::Zero());
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ control_matrix_t dSpldQ0 = controlSpliner_->splineDerivative_q_i((*t_log_)[i], shotIdx_);
+
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ dXdQi_history_.push_back(dXdQi_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdQi_history_.back() + (*B_log_)[i] * dSpldQ0));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1.0 / 6.0;
+ double b2 = 1.0 / 3.0;
+ double b3 = 1.0 / 3.0;
+ double b4 = 1.0 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ assert(i * m < A_log_->size());
+
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_control_matrix_t dK1dQi =
+ (*A_log_)[i * m] * dXdQi_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_control_matrix_t dK2dQi =
+ (*A_log_)[i * m + 1] * (dXdQi_history_.back() + h * a_21 * dK1dQi) +
+ (*B_log_)[i * m + 1] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_control_matrix_t dK3dQi =
+ (*A_log_)[i * m + 2] * (dXdQi_history_.back() + h * a_32 * dK2dQi) +
+ (*B_log_)[i * m + 2] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_control_matrix_t dK4dQi =
+ (*A_log_)[i * m + 3] * (dXdQi_history_.back() + h * a_43 * dK3dQi) +
+ (*B_log_)[i * m + 3] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 3], shotIdx_);
+
+
+ // summation
+ state_control_matrix_t temp =
+ dXdQi_history_.back() + h * (b1 * dK1dQi + b2 * dK2dQi + b3 * dK3dQi + b4 * dK4dQi);
+
+ dXdQi_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. q_(i+1)
+ void compute_dXdQip1()
+ {
+ dXdQip1_history_.clear();
+ dXdQip1_history_.push_back(state_control_matrix_t::Zero());
+
+ assert(t_log_->size() == A_log_->size() + 1);
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ control_matrix_t dSpldQ1 = controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i], shotIdx_);
+
+ assert(dSpldQ1 == dSpldQ1);
+
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ dXdQip1_history_.push_back(
+ dXdQip1_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdQip1_history_.back() + (*B_log_)[i] * dSpldQ1));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1 / 6.0;
+ double b2 = 1 / 3.0;
+ double b3 = 1 / 3.0;
+ double b4 = 1 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_control_matrix_t dK1dQip1 =
+ (*A_log_)[i * m] * dXdQip1_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_control_matrix_t dK2dQip1 =
+ (*A_log_)[i * m + 1] * (dXdQip1_history_.back() + h * a_21 * dK1dQip1) +
+ (*B_log_)[i * m + 1] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_control_matrix_t dK3dQip1 =
+ (*A_log_)[i * m + 2] * (dXdQip1_history_.back() + h * a_32 * dK2dQip1) +
+ (*B_log_)[i * m + 2] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_control_matrix_t dK4dQip1 =
+ (*A_log_)[i * m + 3] * (dXdQip1_history_.back() + h * a_43 * dK3dQip1) +
+ (*B_log_)[i * m + 3] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 3], shotIdx_);
+
+
+ // summation
+ state_control_matrix_t temp =
+ dXdQip1_history_.back() + h * (b1 * dK1dQip1 + b2 * dK2dQip1 + b3 * dK3dQip1 + b4 * dK4dQip1);
+
+ dXdQip1_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error(
+ "... ERROR: sensitivity calculation not implemented for this integration type."));
+ }
+ }
+ }
+
+ void compute_dXdHi()
+ {
+ dXdHi_history_.clear();
+ // shotContainer_->getControlledSystemPtr()->computeDynamics(x_log_->back(), t_log_->back(), dynamics);
+ dXdHi_history_.push_back(Eigen::VectorXd(STATE_DIM).setZero());
+
+ assert(t_log_->size() == A_log_->size() + 1);
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+ dXdHi_history_.push_back(
+ dXdHi_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdHi_history_.back() +
+ (*B_log_)[i] * controlSpliner_->splineDerivative_h_i((*t_log_)[i], shotIdx_)));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1 / 6.0;
+ double b2 = 1 / 3.0;
+ double b3 = 1 / 3.0;
+ double b4 = 1 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_vector_t dK1dHi =
+ (*A_log_)[i * m] * dXdHi_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_vector_t dK2dHi =
+ (*A_log_)[i * m + 1] * (dXdHi_history_.back() + h * a_21 * dK1dHi) +
+ (*B_log_)[i * m + 1] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_vector_t dK3dHi =
+ (*A_log_)[i * m + 2] * (dXdHi_history_.back() + h * a_32 * dK2dHi) +
+ (*B_log_)[i * m + 2] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_vector_t dK4dHi =
+ (*A_log_)[i * m + 3] * (dXdHi_history_.back() + h * a_43 * dK3dHi) +
+ (*B_log_)[i * m + 3] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 3], shotIdx_);
+
+ // summation
+ state_vector_t temp =
+ dXdHi_history_.back() + h * (b1 * dK1dHi + b2 * dK2dHi + b3 * dK3dHi + b4 * dK4dHi);
+
+ dXdHi_history_.push_back(temp);
+ }
+
+ break;
+ }
+
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+ void getdXdSiTraj(state_matrix_array_t& dXdSiTraj) { dXdSiTraj = dXdSi_history_; }
+ void getdXdQiTraj(state_control_matrix_array_t& dXdQiTraj) { dXdQiTraj = dXdQi_history_; }
+ void getdXdQip1Traj(state_control_matrix_array_t& dXdQip1Traj) { dXdQip1Traj = dXdQip1_history_; }
+ void getdXdHiTraj(state_vector_array_t& dXdHiTraj) { dXdHiTraj = dXdHi_history_; }
+private:
+ std::shared_ptr<SplinerBase<control_vector_t>> controlSpliner_;
+ const size_t shotIdx_;
+ const DmsSettings settings_;
+
+ std::shared_ptr<state_vector_array_t> x_log_;
+ std::shared_ptr<control_vector_array_t> u_log_;
+ std::shared_ptr<time_array_t> t_log_;
+ std::shared_ptr<state_matrix_array_t> A_log_;
+ std::shared_ptr<state_control_matrix_array_t> B_log_;
+
+ state_matrix_array_t dXdSi_history_;
+ state_control_matrix_array_t dXdQi_history_;
+ state_control_matrix_array_t dXdQip1_history_;
+ state_vector_array_t dXdHi_history_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h b/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h
new file mode 100644
index 0000000..a006592
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h
@@ -0,0 +1,526 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @brief This class can integrate a controlled system and a costfunction.
+ * Furthermore, it provides first order derivatives with respect to
+ * initial state and control
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class SensitivityIntegratorCT
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector;
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> state_control_matrix;
+
+
+ /**
+ * @brief Constructor
+ *
+ * @param[in] system The controlled system
+ * @param[in] stepperType The integration stepper type
+ */
+ SensitivityIntegratorCT(const std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& system,
+ const ct::core::IntegrationType stepperType = ct::core::IntegrationType::EULERCT)
+ : cacheData_(false), cacheSensitivities_(false)
+ {
+ setControlledSystem(system);
+ initializeDerived(stepperType);
+ }
+
+ /**
+ * @brief Destroys the object.
+ */
+ ~SensitivityIntegratorCT() {}
+ /**
+ * @brief Initializes the steppers
+ *
+ * @param[in] stepperType The desired integration stepper type
+ */
+ void initializeDerived(const ct::core::IntegrationType stepperType)
+ {
+ switch (stepperType)
+ {
+ case ct::core::IntegrationType::EULERCT:
+ {
+ stepperState_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_vector, SCALAR>());
+ stepperDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_matrix, SCALAR>());
+ stepperDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_control_matrix, SCALAR>());
+ stepperCost_ = std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<SCALAR, SCALAR>());
+ stepperCostDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_vector, SCALAR>());
+ stepperCostDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<control_vector, SCALAR>());
+ break;
+ }
+
+ case ct::core::IntegrationType::RK4CT:
+ {
+ stepperState_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_vector, SCALAR>());
+ stepperDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_matrix, SCALAR>());
+ stepperDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_control_matrix, SCALAR>());
+ stepperCost_ = std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<SCALAR, SCALAR>());
+ stepperCostDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_vector, SCALAR>());
+ stepperCostDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<control_vector, SCALAR>());
+ break;
+ }
+
+ default:
+ throw std::runtime_error("Invalid CT integration type");
+ }
+ }
+
+ /**
+ * @brief Prepares the integrator to provide first order sensitivity
+ * generation by setting a linearsystem, enabling state caching
+ * and settings up the function objects
+ *
+ * @param[in] linearSystem The linearized system
+ */
+ void setLinearSystem(const std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& linearSystem)
+ {
+ linearSystem_ = linearSystem;
+ cacheData_ = true;
+
+ dX0dot_ = [this](const state_matrix& dX0In, state_matrix& dX0dt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydX0_.push_back(dX0In);
+
+ dX0dt = arrayA_[dX0Index_] * dX0In;
+ dX0Index_++;
+ };
+
+ dU0dot_ = [this](const state_control_matrix& dU0In, state_control_matrix& dU0dt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydU0_.push_back(dU0In);
+
+ dU0dt = arrayA_[dU0Index_] * dU0In +
+ arrayB_[dU0Index_] *
+ controlledSystem_->getController()->getDerivativeU0(
+ statesCached_[dU0Index_], timesCached_[dU0Index_]);
+ dU0Index_++;
+ };
+
+ dUfdot_ = [this](const state_control_matrix& dUfIn, state_control_matrix& dUfdt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydUf_.push_back(dUfIn);
+
+ dUfdt = arrayA_[dU0Index_] * dUfIn +
+ arrayB_[dU0Index_] *
+ controlledSystem_->getController()->getDerivativeUf(
+ statesCached_[dU0Index_], timesCached_[dU0Index_]);
+ dU0Index_++;
+ };
+ }
+
+ /**
+ * @brief Changes the controlledsystem to be integrated
+ *
+ * @param[in] controlledSystem The new controlled system
+ */
+ void setControlledSystem(
+ const std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& controlledSystem)
+ {
+ controlledSystem_ = controlledSystem;
+ xDot_ = [this](const state_vector& x, state_vector& dxdt, const SCALAR t) {
+ control_vector controlAction;
+ controlledSystem_->getController()->computeControl(x, t, controlAction);
+
+ if (cacheData_)
+ {
+ statesCached_.push_back(x);
+ controlsCached_.push_back(controlAction);
+ timesCached_.push_back(t);
+ }
+
+ controlledSystem_->computeControlledDynamics(x, t, controlAction, dxdt);
+ };
+ }
+
+ /**
+ * @brief Prepares the integrator to provide cost integration and first
+ * order cost derivatives. This is done by enabling sensitivity
+ * caching and setting up the function objects
+ *
+ * @param[in] costFun The new costfunction
+ */
+ void setCostFunction(const std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun)
+ {
+ costFunction_ = costFun;
+ cacheSensitivities_ = true;
+ costDot_ = [this](const SCALAR& costIn, SCALAR& cost, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ cost = costFunction_->evaluateIntermediate();
+ costIndex_++;
+ };
+
+ costdX0dot_ = [this](const state_vector& costdX0In, state_vector& costdX0dt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdX0dt = arraydX0_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate();
+ costIndex_++;
+ };
+
+ costdU0dot_ = [this](const control_vector& costdU0In, control_vector& costdU0dt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdU0dt = arraydU0_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate() +
+ controlledSystem_->getController()->getDerivativeU0(
+ statesCached_[costIndex_], timesCached_[costIndex_]) *
+ costFunction_->controlDerivativeIntermediate();
+ costIndex_++;
+ };
+
+ costdUfdot_ = [this](const control_vector& costdUfIn, control_vector& costdUfdt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdUfdt = arraydUf_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate() +
+ controlledSystem_->getController()->getDerivativeUf(
+ statesCached_[costIndex_], timesCached_[costIndex_]) *
+ costFunction_->controlDerivativeIntermediate();
+ costIndex_++;
+ };
+ }
+
+ /**
+ * @brief Integrates the system starting from state and startTime
+ * for numSteps integration steps. Returns the full state
+ * and time trajectories
+ *
+ * @param[in, out] state The initial state for integration
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number steps
+ * @param[in] dt The integration timestep
+ * @param[out] stateTrajectory The output state trajectory
+ * @param[out] timeTrajectory The output time trajectory
+ */
+ void integrate(state_vector& state,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
+ ct::core::tpl::TimeArray<SCALAR>& timeTrajectory)
+ {
+ clearStates();
+ clearLinearization();
+ cacheData_ = true;
+ stateTrajectory.clear();
+ timeTrajectory.clear();
+ SCALAR time = startTime;
+ stateTrajectory.push_back(state);
+ timeTrajectory.push_back(time);
+
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperState_->do_step(xDot_, state, time, dt);
+ time += dt;
+ stateTrajectory.push_back(state);
+ timeTrajectory.push_back(time);
+ }
+ }
+
+ /**
+ * @brief Integrates the system starting from state and startTime
+ * for numSteps integration steps. Returns only the final
+ * state and time
+ *
+ * @param[int, out] state The initial state for integration
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number steps
+ * @param[in] dt The integration timestep
+ */
+ void integrate(state_vector& state, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ clearStates();
+ clearLinearization();
+ SCALAR time = startTime;
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperState_->do_step(xDot_, state, time, dt);
+ time += dt;
+ }
+ }
+
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the initial state x0
+ *
+ * @param[in, out] dX0 The sensitivity matrix wrt x0
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDX0(state_matrix& dX0, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ dX0Index_ = 0;
+ SCALAR time = startTime;
+ dX0.setIdentity();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDX0_->do_step(dX0dot_, dX0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the initial control input u0
+ *
+ * @param[in, out] dU0 The sensitivity matrix wrt u0
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDU0(state_control_matrix& dU0,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ dU0Index_ = 0;
+ SCALAR time = startTime;
+ dU0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDU0_->do_step(dU0dot_, dU0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the final control input uf
+ *
+ * @param[in, out] dUF The sensitivity matrix wrt uF
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDUf(state_control_matrix& dUf,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ dU0Index_ = 0;
+ SCALAR time = startTime;
+ dUf.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDU0_->do_step(dUfdot_, dUf, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the costfunction using the states and controls
+ * from the costintegration
+ *
+ * @param[in, out] cost The initial cost
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCost(SCALAR& cost, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ SCALAR time = startTime;
+ costIndex_ = 0;
+ if (statesCached_.size() == 0 || controlsCached_.size() == 0 || timesCached_.size() == 0)
+ throw std::runtime_error("States cached are empty");
+
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCost_->do_step(costDot_, cost, time, dt);
+ time += dt;
+ }
+ }
+
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the initial state x0
+ *
+ * @param[in, out] dX0 The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDX0(state_vector& dX0, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dX0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDX0_->do_step(costdX0dot_, dX0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the initial control input u0
+ *
+ * @param[in, out] dU0 The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDU0(control_vector& dU0,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dU0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDU0_->do_step(costdU0dot_, dU0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the final control input uF
+ *
+ * @param[in, out] dUf The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDUf(control_vector& dUf,
+ const SCALAR& startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dUf.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDU0_->do_step(costdUfdot_, dUf, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Linearizes the system around the rollout from the state
+ * interation
+ */
+ void linearize()
+ {
+ for (size_t i = 0; i < statesCached_.size(); ++i)
+ {
+ arrayA_.push_back(linearSystem_->getDerivativeState(statesCached_[i], controlsCached_[i], timesCached_[i]));
+ arrayB_.push_back(
+ linearSystem_->getDerivativeControl(statesCached_[i], controlsCached_[i], timesCached_[i]));
+ }
+ }
+
+ /**
+ * @brief Clears the cached states, controls and times
+ */
+ void clearStates()
+ {
+ statesCached_.clear();
+ controlsCached_.clear();
+ timesCached_.clear();
+ }
+
+
+ /**
+ * @brief Clears the cached sensitivities
+ */
+ void clearSensitivities()
+ {
+ arraydX0_.clear();
+ arraydU0_.clear();
+ arraydUf_.clear();
+ }
+
+
+ /**
+ * @brief Clears the linearized matrices
+ */
+ void clearLinearization()
+ {
+ arrayA_.clear();
+ arrayB_.clear();
+ }
+
+private:
+ std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem_;
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem_;
+ std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction_;
+
+ // Integrate the function
+ std::function<void(const SCALAR, SCALAR&, const SCALAR)> costDot_;
+ std::function<void(const state_vector&, state_vector&, const SCALAR)> costdX0dot_;
+ std::function<void(const control_vector&, control_vector&, const SCALAR)> costdU0dot_;
+ std::function<void(const control_vector&, control_vector&, const SCALAR)> costdUfdot_;
+
+ // Sensitivities
+ std::function<void(const state_matrix&, state_matrix&, const SCALAR)> dX0dot_;
+ std::function<void(const state_control_matrix&, state_control_matrix&, const SCALAR)> dU0dot_;
+ std::function<void(const state_control_matrix&, state_control_matrix&, const SCALAR)> dUfdot_;
+
+ // Cache
+ bool cacheData_;
+ bool cacheSensitivities_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> statesCached_;
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> controlsCached_;
+ ct::core::tpl::TimeArray<SCALAR> timesCached_;
+
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> arrayA_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arrayB_;
+
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> arraydX0_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arraydU0_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arraydUf_;
+
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>> stepperState_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>> stepperDX0_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>> stepperDU0_;
+
+ std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>> stepperCost_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>> stepperCostDX0_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>> stepperCostDU0_;
+
+ size_t costIndex_;
+ size_t dX0Index_;
+ size_t dU0Index_;
+
+ std::function<void(const state_vector&, state_vector&, const SCALAR)> xDot_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h b/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h
new file mode 100644
index 0000000..76f02e9
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h
@@ -0,0 +1,360 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <functional>
+
+#include "SensitivityIntegratorCT.h"
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class performs the state and the sensitivity integration on
+ * a shot
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ShotContainer
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ ShotContainer() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] controlledSystem The nonlinear system
+ * @param[in] linearSystem The linearized system
+ * @param[in] costFct The costfunction
+ * @param[in] w The optimization vector
+ * @param[in] controlSpliner The control input spliner
+ * @param[in] timeGrid The timegrid
+ * @param[in] shotNr The shot number
+ * @param[in] settings The dms settings
+ */
+ ShotContainer(std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem,
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem,
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ size_t shotNr,
+ DmsSettings settings,
+ size_t nIntegrationSteps)
+ : controlledSystem_(controlledSystem),
+ linearSystem_(linearSystem),
+ costFct_(costFct),
+ w_(w),
+ controlSpliner_(controlSpliner),
+ timeGrid_(timeGrid),
+ shotNr_(shotNr),
+ settings_(settings),
+ integrationCount_(0),
+ costIntegrationCount_(0),
+ sensIntegrationCount_(0),
+ costSensIntegrationCount_(0),
+ cost_(SCALAR(0.0)),
+ discreteQ_(state_vector_t::Zero()),
+ discreteR_(control_vector_t::Zero()),
+ discreteRNext_(control_vector_t::Zero())
+ {
+ if (shotNr_ >= settings.N_)
+ throw std::runtime_error("Dms Shot Integrator: shot index >= settings.N_ - check your settings.");
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ integratorCT_ = std::allocate_shared<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>,
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>>(
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>(),
+ controlledSystem_, core::EULERCT);
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ integratorCT_ = std::allocate_shared<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>,
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>>(
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>(),
+ controlledSystem_, core::RK4CT);
+ break;
+ }
+ case DmsSettings::RK5:
+ {
+ throw std::runtime_error("Currently we do not support adaptive integrators in dms");
+ }
+
+ default:
+ {
+ std::cerr << "... ERROR: unknown integration type. Exiting" << std::endl;
+ exit(0);
+ }
+ }
+
+ tStart_ = timeGrid_->getShotStartTime(shotNr_);
+ // SCALAR t_shot_end = timeGrid_->getShotEndTime(shotNr_);
+
+ // +0.5 needed to avoid rounding errors from double to size_t
+ nSteps_ = nIntegrationSteps;
+ // std::cout << "shotNr_: " << shotNr_ << "\t nSteps: " << nSteps_ << std::endl;
+
+ integratorCT_->setLinearSystem(linearSystem_);
+
+ if (settings_.costEvaluationType_ == DmsSettings::FULL)
+ integratorCT_->setCostFunction(costFct_);
+ }
+
+ /**
+ * @brief Performs the state integration between the shots
+ */
+ void integrateShot()
+ {
+ if ((w_->getUpdateCount() != integrationCount_))
+ {
+ integrationCount_ = w_->getUpdateCount();
+ state_vector_t initState = w_->getOptimizedState(shotNr_);
+ integratorCT_->integrate(
+ initState, tStart_, nSteps_, SCALAR(settings_.dt_sim_), stateSubsteps_, timeSubsteps_);
+ }
+ }
+
+ void integrateCost()
+ {
+ if ((w_->getUpdateCount() != costIntegrationCount_))
+ {
+ costIntegrationCount_ = w_->getUpdateCount();
+ integrateShot();
+ cost_ = SCALAR(0.0);
+ integratorCT_->integrateCost(cost_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+
+ /**
+ * @brief Performs the state and the sensitivity integration between the shots
+ */
+ void integrateSensitivities()
+ {
+ if ((w_->getUpdateCount() != sensIntegrationCount_))
+ {
+ sensIntegrationCount_ = w_->getUpdateCount();
+ integrateShot();
+ discreteA_.setIdentity();
+ discreteB_.setZero();
+ integratorCT_->linearize();
+ integratorCT_->integrateSensitivityDX0(discreteA_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ integratorCT_->integrateSensitivityDU0(discreteB_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+
+ if (settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ {
+ discreteBNext_.setZero();
+ integratorCT_->integrateSensitivityDUf(discreteBNext_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+ }
+
+ void integrateCostSensitivities()
+ {
+ if ((w_->getUpdateCount() != costSensIntegrationCount_))
+ {
+ costSensIntegrationCount_ = w_->getUpdateCount();
+ integrateSensitivities();
+ discreteQ_.setZero();
+ discreteR_.setZero();
+ integratorCT_->integrateCostSensitivityDX0(discreteQ_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ integratorCT_->integrateCostSensitivityDU0(discreteR_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+
+ if (settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ {
+ discreteRNext_.setZero();
+ integratorCT_->integrateCostSensitivityDUf(discreteRNext_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+ }
+
+ void reset()
+ {
+ integratorCT_->clearStates();
+ integratorCT_->clearSensitivities();
+ integratorCT_->clearLinearization();
+ }
+
+ /**
+ * @brief Returns the integrated state
+ *
+ * @return The integrated state
+ */
+ const state_vector_t getStateIntegrated() { return stateSubsteps_.back(); }
+ /**
+ * @brief Returns the end time of the integration
+ *
+ * @return The end time of the integration.
+ */
+ const SCALAR getIntegrationTimeFinal() { return timeSubsteps_.back(); }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized state s_i
+ *
+ * @return The integrated sensitivity
+ */
+ const state_matrix_t getdXdSiIntegrated() { return discreteA_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized inputs q_i
+ *
+ * @return The integrated sensitivity
+ */
+ const state_control_matrix_t getdXdQiIntegrated() { return discreteB_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized inputs q_{i+1}
+ *
+ * @return The integrated sensitivity
+ */
+ const state_control_matrix_t getdXdQip1Integrated() { return discreteBNext_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * time segments h_i
+ *
+ * @return The integrated sensitivity
+ */
+ // const state_vector_t getdXdHiIntegrated()
+ // {
+ // return dXdHi_history_.back();
+ // }
+
+ /**
+ * @brief Gets the full integrated state trajectory.
+ *
+ * @return The integrated state trajectory
+ */
+ const state_vector_array_t& getXHistory() const { return stateSubsteps_; }
+ /**
+ * @brief Returns the control input trajectory used during the state integration
+ *
+ * @return The control trajectory
+ */
+ const control_vector_array_t& getUHistory()
+ {
+ inputSubsteps_.clear();
+ for (size_t t = 0; t < timeSubsteps_.size(); ++t)
+ {
+ inputSubsteps_.push_back(controlSpliner_->evalSpline(timeSubsteps_[t], shotNr_));
+ }
+ return inputSubsteps_;
+ }
+
+ /**
+ * @brief Returns the time trajectory used during the integration
+ *
+ * @return The time trajectory
+ */
+ const time_array_t& getTHistory() const { return timeSubsteps_; }
+ /**
+ * @brief Gets the cost integrated.
+ *
+ * @return The integrated cost.
+ */
+ const SCALAR getCostIntegrated() const { return cost_; }
+ /**
+ * @brief Returns the cost gradient with respect to s_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ const state_vector_t getdLdSiIntegrated() const { return discreteQ_; }
+ /**
+ * @brief Returns the cost gradient with respect to q_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ const control_vector_t getdLdQiIntegrated() const { return discreteR_; }
+ /**
+ * @brief Returns to cost gradient with respect to q_{i+1} integrated
+ * over the shot
+ *
+ * @return The cost gradient
+ */
+ const control_vector_t getdLdQip1Integrated() const { return discreteRNext_; }
+ /**
+ * @brief Returns to cost gradient with respect to h_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ // const double getdLdHiIntegrated() const
+ // {
+ // return costGradientHi_;
+ // }
+
+
+private:
+ std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem_;
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem_;
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+
+ const size_t shotNr_;
+ const DmsSettings settings_;
+
+ size_t integrationCount_;
+ size_t costIntegrationCount_;
+ size_t sensIntegrationCount_;
+ size_t costSensIntegrationCount_;
+
+ // Integrated trajectories
+ state_vector_array_t stateSubsteps_;
+ control_vector_array_t inputSubsteps_;
+ time_array_t timeSubsteps_;
+
+ //Sensitivity Trajectories
+ state_matrix_t discreteA_;
+ state_control_matrix_t discreteB_;
+ state_control_matrix_t discreteBNext_;
+
+ //Cost and cost gradient
+ SCALAR cost_;
+ state_vector_t discreteQ_;
+ control_vector_t discreteR_;
+ control_vector_t discreteRNext_;
+
+ std::shared_ptr<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>> integratorCT_;
+ size_t nSteps_;
+ SCALAR tStart_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h b/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h
new file mode 100644
index 0000000..b7b5c22
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h
@@ -0,0 +1,161 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+//#define DEBUG_TIMEGRID
+
+#include <math.h>
+#include <cmath>
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup DMS
+ *
+ * This class describes the time-grid underlying the shots in the dms problem In
+ * total, we have N+1 pairs of (s_i, q_i) and N shots between them.
+ *
+ * We assume that starting time t_0 = 0.0 [sec]
+ */
+template <typename SCALAR>
+class TimeGrid
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ TimeGrid() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] numberOfShots The number of shots
+ * @param[in] timeHorizon The dms time horizon
+ */
+ TimeGrid(const size_t numberOfShots, const SCALAR timeHorizon)
+ : numberOfShots_(numberOfShots), timeHorizon_(timeHorizon), t_(numberOfShots + 1, SCALAR(0.0))
+ {
+ makeUniformGrid();
+ }
+
+ /**
+ * @brief Updates the timegrid when the number of shots changes
+ *
+ * @param[in] numberOfShots The new number of shots
+ */
+ void changeShotCount(const size_t numberOfShots)
+ {
+ numberOfShots_ = numberOfShots;
+ t_.clear();
+ t_.resize(numberOfShots_ + 1, SCALAR(0.0));
+ makeUniformGrid();
+ }
+
+ /**
+ * @brief Updates the timegrid when the timehorizon changes
+ *
+ * @param[in] timeHorizon The new time horizon
+ */
+ void changeTimeHorizon(const SCALAR timeHorizon)
+ {
+ timeHorizon_ = timeHorizon;
+ makeUniformGrid();
+ }
+
+
+ /**
+ * @brief This method updates the timegrid when new optimized time
+ * segments arrive from the nlp solver. Only gets called when
+ * using timegrid optimization, otherwise the timegrid stays
+ * fixed
+ *
+ * @param[in] h_segment The vector of the new optimized time segments
+ */
+ void updateTimeGrid(const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& h_segment)
+ {
+ t_[0] = SCALAR(0.0); //by convention (just for documentation)
+
+ for (size_t i = 0; i < (size_t)h_segment.size(); ++i)
+ t_[i + 1] = t_[i] + h_segment(i);
+
+#ifdef DEBUG_TIMEGRID
+ std::cout << " ... in updateTimeGrid(). t_ = ";
+ for (size_t i = 0; i < t_.size(); i++)
+ std::cout << std::setprecision(10) << t_[i] << " ";
+
+ std::cout << std::endl;
+#endif
+ }
+
+
+ /**
+ * @brief Creates a uniform timegrid
+ */
+ void makeUniformGrid()
+ {
+ for (size_t i = 0; i < numberOfShots_ + 1; i++)
+ t_[i] = i * (SCALAR)(timeHorizon_ / (SCALAR)numberOfShots_);
+ }
+
+
+ /**
+ * @brief Returns to start time of a shot
+ *
+ * @param[in] shot_index The shot number
+ *
+ * @return The start time
+ */
+ const SCALAR getShotStartTime(const size_t shot_index) const { return t_[shot_index]; }
+ /**
+ * @brief Returns the end time of a shot
+ *
+ * @param[in] shot_index The shot number
+ *
+ * @return The end time
+ */
+ const SCALAR getShotEndTime(const size_t shot_index) const { return t_[shot_index + 1]; }
+ /**
+ * @brief Returns to duration of a shot
+ *
+ * @param[in] shot_index The shot index
+ *
+ * @return The duration
+ */
+ const SCALAR getShotDuration(const size_t shot_index) const { return (t_[shot_index + 1] - t_[shot_index]); }
+ /**
+ * @brief Returns the underlying TimeArray
+ *
+ * @return The underlying TimeArray
+ */
+ const ct::core::tpl::TimeArray<SCALAR>& toImplementation() { return t_; }
+ /**
+ * @brief Returns the initial timehorizon of the problem
+ *
+ * @return The initial time horizon
+ */
+ const SCALAR getTimeHorizon() const { return timeHorizon_; }
+ /**
+ * @brief Returns the optimized timehorizon
+ *
+ * @return The optimized timehorizon
+ */
+ const SCALAR getOptimizedTimeHorizon() const { return t_.back(); }
+private:
+ size_t numberOfShots_;
+ SCALAR timeHorizon_;
+
+ // the individual times of each pair from i=0,..., N
+ ct::core::tpl::TimeArray<SCALAR> t_;
+};
+}
+
+typedef tpl::TimeGrid<double> TimeGrid;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
new file mode 100644
index 0000000..43d2a76
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
@@ -0,0 +1,152 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <omp.h>
+#include <math.h>
+#include <cmath>
+#include <functional>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+#include <ct/optcon/nlp/DiscreteCostEvaluatorBase.h>
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Performs the full cost integration over the shots
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostEvaluatorFull : public tpl::DiscreteCostEvaluatorBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ CostEvaluatorFull() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] costFct The cost function
+ * @param[in] w The optimization vector
+ * @param[in] controlSpliner The control spliner
+ * @param[in] shotInt The shot number
+ * @param[in] settings The dms settings
+ */
+ CostEvaluatorFull(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotInt,
+ DmsSettings settings)
+ : costFct_(costFct), w_(w), controlSpliner_(controlSpliner), shotContainers_(shotInt), settings_(settings)
+ {
+ }
+
+ /**
+ * @brief The destructor.
+ */
+ virtual ~CostEvaluatorFull() {}
+ virtual SCALAR eval() override
+ {
+ SCALAR cost = SCALAR(0.0);
+
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateCost();
+ }
+
+ for (auto shotContainer : shotContainers_)
+ cost += shotContainer->getCostIntegrated();
+
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ cost += costFct_->evaluateTerminal();
+ return cost;
+ }
+
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) override
+ {
+ grad.setZero();
+
+ assert(shotContainers_.size() == settings_.N_);
+
+// go through all shots, integrate the state trajectories and evaluate cost accordingly
+// intermediate costs
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateCostSensitivities();
+ }
+
+ for (size_t shotNr = 0; shotNr < shotContainers_.size(); ++shotNr)
+ {
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ grad.segment(w_->getStateIndex(shotNr), STATE_DIM) += shotContainers_[shotNr]->getdLdSiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQiIntegrated();
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ grad.segment(w_->getStateIndex(shotNr), STATE_DIM) += shotContainers_[shotNr]->getdLdSiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr + 1), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQip1Integrated();
+ break;
+ }
+ default:
+ throw(std::runtime_error(
+ " cost gradient not yet implemented for this type of interpolation. Exiting"));
+ }
+
+ // H-part.
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // costFct_->setCurrentStateAndControl(shotContainers_[shotNr]->getStateIntegrated(),
+ // controlSpliner_->evalSpline(shotContainers_[shotNr]->getIntegrationTimeFinal(), shotNr));
+ // grad(w_->getTimeSegmentIndex(shotNr)) = costFct_->evaluateIntermediate() + shotContainers_[shotNr]->getdLdHiIntegrated();
+ // }
+ }
+
+ /* gradient of terminal cost */
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ grad.segment(w_->getStateIndex(settings_.N_), STATE_DIM) +=
+ costFct_->stateDerivativeTerminal(); // * dXdSi.back();
+ }
+
+private:
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+
+ const DmsSettings settings_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
new file mode 100644
index 0000000..d2b32f7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
@@ -0,0 +1,172 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <omp.h>
+#include <math.h>
+#include <cmath>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+#include <ct/optcon/nlp/DiscreteCostEvaluatorBase.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Evaluates the cost at the shots and performs some interpolation
+ * in between
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostEvaluatorSimple : public tpl::DiscreteCostEvaluatorBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+ CostEvaluatorSimple() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] costFct The cost function
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The time grid
+ * @param[in] settings The dms settings
+ */
+ CostEvaluatorSimple(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ DmsSettings settings)
+ : costFct_(costFct), w_(w), timeGrid_(timeGrid), settings_(settings)
+ {
+ phi_.resize(settings_.N_ + 1);
+ // phi_diff_h_ = Eigen::VectorXd::Ones(settings_.N_+1, 1);
+ updatePhi();
+ }
+
+ virtual ~CostEvaluatorSimple() {}
+ virtual SCALAR eval() override;
+
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) override;
+
+private:
+ /**
+ * @brief Updates the weights for the cost interpolation
+ */
+ void updatePhi();
+
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ const DmsSettings settings_;
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> phi_; /* the summation weights */
+ // Eigen::VectorXd phi_diff_h_; /* the summation weights for derivative w.r.t h */
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::eval()
+{
+ // updatePhi();
+ SCALAR cost = SCALAR(0.0);
+
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ cost += phi_(i) * costFct_->evaluateIntermediate();
+ }
+
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ cost += costFct_->evaluateTerminal();
+ assert(cost == cost);
+ return cost;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evalGradient(size_t grad_length,
+ Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad)
+{
+ // updatePhi();
+ grad.setZero();
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ grad.segment(w_->getStateIndex(i), STATE_DIM) += phi_(i) * costFct_->stateDerivativeIntermediate();
+ grad.segment(w_->getControlIndex(i), CONTROL_DIM) += phi_(i) * costFct_->controlDerivativeIntermediate();
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID && i < settings_.N_)
+ // {
+ // if(settings_.splineType_ == DmsSettings::ZERO_ORDER_HOLD)
+ // {
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ // double dJdH = phi_diff_h_(i) * costFct_->evaluateIntermediate();
+ // size_t idx = w_->getTimeSegmentIndex(i);
+ // grad(idx) = dJdH;
+ // }
+
+ // else if(settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ // {
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ // double dJdH = 0.5 * costFct_->evaluateIntermediate();
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i+1), w_->getOptimizedControl(i+1));
+ // dJdH += 0.5 * costFct_->evaluateIntermediate();
+ // grad(w_->getTimeSegmentIndex(i)) = dJdH;
+ // }
+ // }
+ }
+
+ /* gradient of terminal cost */
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ grad.segment(w_->getStateIndex(settings_.N_), STATE_DIM) += costFct_->stateDerivativeTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updatePhi()
+{
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ for (size_t i = 0; i < settings_.N_; i++)
+ phi_(i) = (timeGrid_->getShotDuration(i));
+
+ phi_(settings_.N_) = SCALAR(0.0);
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ phi_(0) = SCALAR(0.5) * (timeGrid_->getShotDuration(0));
+
+ for (size_t i = 1; i < settings_.N_; i++)
+ phi_(i) = SCALAR(0.5) * (timeGrid_->getShotEndTime(i) - timeGrid_->getShotStartTime(i - 1));
+
+ phi_(settings_.N_) = SCALAR(0.5) * (timeGrid_->getShotDuration(settings_.N_));
+ break;
+ }
+ default:
+ throw(std::runtime_error(" ERROR: Unknown spline-type in CostEvaluatorSimple - exiting."));
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h b/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h
new file mode 100644
index 0000000..bb8e356
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h
@@ -0,0 +1,243 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::OptVectorDms(size_t n, const DmsSettings& settings)
+ : tpl::OptVector<SCALAR>(n), settings_(settings), numPairs_(settings.N_ + 1)
+{
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // optimizedTimeSegments_.resize(numPairs_ - 1);
+
+ size_t currIndex = 0;
+
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ pairNumToStateIdx_.insert(std::make_pair(i, currIndex));
+ currIndex += STATE_DIM;
+
+ pairNumToControlIdx_.insert(std::make_pair(i, currIndex));
+ currIndex += CONTROL_DIM;
+
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // if(i < numPairs_-1)
+ // {
+ // shotNumToShotDurationIdx_.insert(std::make_pair(i, currIndex));
+ // currIndex += 1;
+ // }
+ }
+
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // setLowerTimeSegmentBounds();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedState(const size_t pairNum) const
+{
+ size_t index = getStateIndex(pairNum);
+ return (this->x_.segment(index, STATE_DIM));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedControl(const size_t pairNum) const
+{
+ size_t index = getControlIndex(pairNum);
+ return (this->x_.segment(index, CONTROL_DIM));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedTimeSegment(const size_t pairNum) const
+{
+ size_t index = getTimeSegmentIndex(pairNum);
+ // std::cout << "x_(index) : " << x_(index) << std::endl;
+ return this->x_(index);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_array_t&
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedStates()
+{
+ stateSolution_.clear();
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ stateSolution_.push_back(getOptimizedState(i));
+ }
+ return stateSolution_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_array_t&
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedInputs()
+{
+ inputSolution_.clear();
+ for (size_t i = 0; i < numPairs_; i++)
+ inputSolution_.push_back(getOptimizedControl(i));
+
+ return inputSolution_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedTimeSegments()
+{
+ for (size_t i = 0; i < numPairs_ - 1; ++i)
+ optimizedTimeSegments_(i) = getOptimizedTimeSegment(i);
+
+ return optimizedTimeSegments_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getStateIndex(const size_t pairNum) const
+{
+ return pairNumToStateIdx_.find(pairNum)->second;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getControlIndex(const size_t pairNum) const
+{
+ return pairNumToControlIdx_.find(pairNum)->second;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getTimeSegmentIndex(const size_t shotNr) const
+{
+ return (shotNumToShotDurationIdx_.find(shotNr)->second);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeInitialState(const state_vector_t& x0)
+{
+ size_t s_index = getStateIndex(0);
+ this->x_.segment(s_index, STATE_DIM) = x0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeDesiredState(const state_vector_t& xF)
+{
+ size_t s_index = pairNumToStateIdx_.find(settings_.N_)->second;
+ this->x_.segment(s_index, STATE_DIM) = xF;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setInitGuess(const state_vector_t& x0,
+ const state_vector_t& x_f,
+ const control_vector_t& u0)
+{
+ size_t type = 1; // 0 = constant init guess, 1 = linearly interpolated between x0 and x_f
+
+ // init the states s_i and controls q_i
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ size_t s_index = getStateIndex(i);
+ size_t q_index = getControlIndex(i);
+
+ switch (type)
+ {
+ case 0:
+ {
+ this->xInit_.segment(s_index, STATE_DIM) = x0;
+ break;
+ }
+ case 1:
+ this->xInit_.segment(s_index, STATE_DIM) = x0 + (x_f - x0) * (i / (numPairs_ - 1));
+ break;
+ }
+ this->xInit_.segment(q_index, CONTROL_DIM) = u0;
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // for (size_t i = 0; i< numPairs_ - 1; i++)
+ // {
+ // size_t h_index = getTimeSegmentIndex(i);
+ // x_(h_index) = (double)settings_.T_ / (double)settings_.N_;
+ // }
+ // }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setInitGuess(const state_vector_array_t& x_init,
+ const control_vector_array_t& u_init)
+{
+ if (x_init.size() != numPairs_)
+ throw std::runtime_error("initial guess state trajectory not matching number of shots");
+ if (u_init.size() != numPairs_)
+ throw std::runtime_error("initial guess input trajectory not matching number of shots");
+
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ size_t s_index = getStateIndex(i);
+ size_t q_index = getControlIndex(i);
+
+ this->xInit_.segment(s_index, STATE_DIM) = x_init[i];
+ this->xInit_.segment(q_index, CONTROL_DIM) = u_init[i];
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // for (size_t i = 0; i< numPairs_ - 1; i++)
+ // {
+ // size_t h_index = getTimeSegmentIndex(i);
+ // x_(h_index) = (double)settings_.T_ / (double)settings_.N_;
+ // }
+ // }
+
+ // std::cout << "x_ init: " << x_.transpose() << std::endl;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setLowerTimeSegmentBounds()
+{
+ for (size_t i = 0; i < numPairs_ - 1; i++)
+ {
+ size_t h_index = shotNumToShotDurationIdx_.find(i)->second;
+ Eigen::Matrix<SCALAR, 1, 1> newElement;
+ newElement(0, 0) = settings_.h_min_;
+ this->xLb_.segment(h_index, 1) = newElement; //0.0;
+ newElement(0, 0) = 0.5;
+ this->xUb_.segment(h_index, 1) = newElement;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::printoutSolution()
+{
+ std::cout << "... printing solutions: " << std::endl;
+ std::cout << "x_solution" << std::endl;
+ state_vector_array_t x_sol = getOptimizedStates();
+ for (size_t i = 0; i < x_sol.size(); ++i)
+ {
+ std::cout << x_sol[i].transpose() << std::endl;
+ }
+
+ std::cout << "u_solution" << std::endl;
+ control_vector_array_t u_sol = getOptimizedInputs();
+ for (size_t i = 0; i < u_sol.size(); ++i)
+ {
+ std::cout << u_sol[i].transpose() << std::endl;
+ }
+
+ std::cout << "t_solution" << std::endl;
+ const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& t_sol = getOptimizedTimeSegments();
+ for (size_t i = 0; i < t_sol.size(); ++i)
+ {
+ std::cout << t_sol[i] << " ";
+ }
+ std::cout << std::endl;
+ std::cout << " ... done." << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h
new file mode 100644
index 0000000..d2b365b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h
@@ -0,0 +1,121 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ct/optcon/dms/dms_core/spline/SplinerBase.h"
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The linear spline implementation
+ *
+ * @tparam T The vector type to be splined
+ */
+template <class T, typename SCALAR = double>
+class LinearSpliner : public SplinerBase<T, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef T vector_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+
+ LinearSpliner() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] grid The dms timegrid
+ */
+ LinearSpliner(std::shared_ptr<tpl::TimeGrid<SCALAR>> grid) : timeGrid_(grid) {}
+ virtual ~LinearSpliner() {}
+ void computeSpline(const vector_array_t& points) override { nodes_ = points; }
+ // evaluate spline and return vector at interpolation time
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) override
+ {
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> result;
+ result.resize(T::DIM);
+
+ // int shotIdx = timeGrid_->getShotIndex(time);
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+ SCALAR t_s_end = timeGrid_->getShotEndTime(shotIdx); /* time when this particular shot ends */
+
+ assert(shotIdx < nodes_.size());
+
+ result = nodes_[shotIdx] * (t_s_end - time) / t_shot + nodes_[shotIdx + 1] * (time - t_s_start) / t_shot;
+
+ return result;
+ }
+
+
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const override
+ {
+ vector_t result;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+
+ result = (nodes_[shotIdx + 1] - nodes_[shotIdx]) / t_shot;
+
+ return result;
+ }
+
+
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ vector_t result;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+
+ result = (time - t_s_start) * (nodes_[shotIdx] - nodes_[shotIdx + 1]) / (t_shot * t_shot);
+
+ return result;
+ }
+
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ matrix_t drv;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a the shot*/
+ SCALAR t_s_end = timeGrid_->getShotEndTime(shotIdx); /* time when this particular shot ends */
+
+ drv.setIdentity();
+ drv *= (t_s_end - time) / t_shot;
+
+ return drv;
+ }
+
+
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const override
+ {
+ matrix_t drv;
+
+ // int shotIdx = timeGrid_->getShotIndex(time);
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of the shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+
+ drv.setIdentity();
+ drv *= (time - t_s_start) / t_shot;
+
+ return drv;
+ }
+
+
+private:
+ vector_array_t nodes_; // an array of references to grid points between which is interpolated
+
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h
new file mode 100644
index 0000000..ead9cc0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Abstract base class for the control input splining between the
+ * DMS shots
+ *
+ * @tparam T The vector type which will be splined
+ */
+template <class T, typename SCALAR = double>
+class SplinerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ SplinerBase(){};
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~SplinerBase(){};
+
+ typedef T vector_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+
+
+ /**
+ * @brief Updates the vector on the shots
+ *
+ * @param[in] points Updated vector array
+ */
+ virtual void computeSpline(const vector_array_t& points) = 0;
+
+ /**
+ * @brief Depending on the spline type, this method evaluates the
+ * control input between the shots
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The splined vector
+ */
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) = 0;
+
+ /**
+ * @brief Returns the spline derivatives with respect to time
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The time derivative
+ */
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Returns the spline derivatives with respect to the time
+ * segment between the shots
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Return the spline derivative with respect to the control
+ * input at shot i
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Returns the spline derivative with respect to the control
+ * input at shot i+1
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const = 0;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h
new file mode 100644
index 0000000..dac0ccd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h
@@ -0,0 +1,79 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ct/optcon/dms/dms_core/spline/SplinerBase.h"
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The spline implementation for the zero order hold spliner
+ *
+ * @tparam T The vector type to be splined
+ */
+template <class T, typename SCALAR = double>
+class ZeroOrderHoldSpliner : public SplinerBase<T, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef T vector_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+
+ ZeroOrderHoldSpliner() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] grid The DMS timegrid
+ */
+ ZeroOrderHoldSpliner(std::shared_ptr<tpl::TimeGrid<SCALAR>> grid) : timeGrid_(grid) {}
+ virtual ~ZeroOrderHoldSpliner() {}
+ void computeSpline(const vector_array_t& points) override { zOholds_ = points; }
+ // evaluate spline and return vector at interpolation time
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) override
+ {
+ assert(shotIdx < zOholds_.size());
+ assert(zOholds_[shotIdx] == zOholds_[shotIdx]);
+ return zOholds_[shotIdx];
+ }
+
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const override
+ {
+ return vector_t::Zero();
+ }
+
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ return vector_t::Zero();
+ }
+
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ return matrix_t::Identity();
+ }
+
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const override
+ {
+ return matrix_t::Zero();
+ }
+
+
+private:
+ // zero order hold variables
+ vector_array_t zOholds_;
+
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp b/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
new file mode 100644
index 0000000..b098be2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
@@ -0,0 +1,162 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::FHDTLQR(
+ std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction)
+ : costFunction_(costFunction)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::~FHDTLQR()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ size_t N = x_trajectory.size() - 1;
+ solve(x_trajectory, u_trajectory, A, B, N, dt, K, performNumericalChecks);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> derivatives,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ size_t N = x_trajectory.size() - 1;
+
+ state_matrix_array_t A;
+ control_gain_matrix_array_t B;
+
+ linearizeModel(x_trajectory, u_trajectory, N, dt, derivatives, A, B);
+
+ solve(x_trajectory, u_trajectory, A, B, N, dt, K, performNumericalChecks);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::linearizeModel(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ size_t N,
+ SCALAR dt,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& derivatives,
+ state_matrix_array_t& A,
+ control_gain_matrix_array_t& B)
+{
+ A.resize(N);
+ B.resize(N);
+
+ for (int i = 0; i < N; i++)
+ {
+ A[i] = state_matrix_t::Identity() + dt * derivatives->getDerivativeState(x_trajectory[i], u_trajectory[i]);
+ B[i] = dt * derivatives->getDerivativeControl(x_trajectory[i], u_trajectory[i]);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::solve(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ size_t N,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ if (x_trajectory.size() != N + 1)
+ {
+ throw std::runtime_error("State trajectory does not have correct length. State trajectory length: " +
+ std::to_string(x_trajectory.size()) + ", should be: " + std::to_string(N + 1));
+ }
+ if (u_trajectory.size() != N)
+ {
+ throw std::runtime_error("Input trajectory does not have correct length. Input trajectory length: " +
+ std::to_string(u_trajectory.size()) + ", should be: " + std::to_string(N));
+ }
+ if (A.size() != N)
+ {
+ throw std::runtime_error("Linearization A does not have correct length. Linearization length: " +
+ std::to_string(A.size()) + ", should be: " + std::to_string(N));
+ }
+ if (B.size() != N)
+ {
+ throw std::runtime_error("Linearization B does not have correct length. Linearization length: " +
+ std::to_string(B.size()) + ", should be: " + std::to_string(N));
+ }
+
+ K.resize(N);
+
+ // initialize cost-to-go
+ costFunction_->setCurrentStateAndControl(x_trajectory[N], control_vector_t::Zero(), dt * N);
+ state_matrix_t P = costFunction_->stateSecondDerivativeTerminal();
+
+ if (performNumericalChecks)
+ {
+ Eigen::Matrix<SCALAR, -1, 1> realEigVals = P.eigenvalues().real();
+ if (realEigVals.minCoeff() < 0.0)
+ {
+ std::cout << "P: " << std::endl << P;
+ throw std::runtime_error("[LQR] Q is not positive semi-definite.");
+ }
+ }
+
+ for (int i = N - 1; i >= 0; i--)
+ {
+ costFunction_->setCurrentStateAndControl(x_trajectory[i], u_trajectory[i], i * dt);
+
+ state_matrix_t Q = costFunction_->stateSecondDerivativeIntermediate() * dt;
+
+ control_matrix_t R = costFunction_->controlSecondDerivativeIntermediate() * dt;
+
+ if (performNumericalChecks)
+ {
+ if (Q.minCoeff() < 0.0)
+ {
+ std::cout << "Q: " << std::endl << Q;
+ throw std::runtime_error("[LQR] Q contains negative entries.");
+ }
+ if (R.minCoeff() < 0.0)
+ {
+ std::cout << "R: " << std::endl << R;
+ throw std::runtime_error("[LQR] R contains negative entries.");
+ }
+ if (R.diagonal().minCoeff() <= 0.0)
+ {
+ std::cout << "R: " << std::endl << R;
+ throw std::runtime_error("[LQR] R contains zero entries on the diagonal.");
+ }
+ if (!Q.isDiagonal())
+ {
+ std::cout << "[LQR] Warning: Q is not diagonal." << std::endl;
+ }
+ if (!R.isDiagonal())
+ {
+ std::cout << "[LQR] Warning: R is not diagonal." << std::endl;
+ }
+ }
+
+ //ricattiEq_.iterateNaive(Q, R, A, B, P, K[i]);
+ ricattiEq_.iterateRobust(Q, R, A[i], B[i], P, K[i]);
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp b/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp
new file mode 100644
index 0000000..57f99bb
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp
@@ -0,0 +1,182 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp>
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+namespace ct {
+namespace optcon {
+
+/*! \defgroup LQR LQR
+ *
+ * \brief Linear Quadratic Regulator Module
+ * This module holds two verified LQR implementations in C++.
+ */
+
+/*!
+ * \ingroup LQR
+ *
+ * \brief Finite-Horizon Discrete Time LQR
+ *
+ * compute the finite-horizon discrete time LQR solution
+ * (Example: stabilize a linear time-varying system about a trajectory).
+ * The user can either provide the linearized system matrices, or alternatively a pointer to a derivatives instance
+ *
+ * The feedback law has form
+ * \f[
+ * u_{fb} = -K \cdot (x - x_{ref})
+ * \f]
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class FHDTLQR
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ typedef core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+ typedef core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> state_matrix_array_t;
+
+ typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> control_feedback_array_t;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> control_gain_matrix_array_t;
+
+
+ //! Constructor
+ /*!
+ * @param costFunction the cost function to be used for designing the TVLQR
+ */
+ FHDTLQR(std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction);
+
+ ~FHDTLQR();
+
+
+ //! design a time-varying feedback trajectory using user-provided matrices A and B
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param A
+ * trajectory of system matrices A = df/dx
+ * @param B
+ * trajectory of system matrices B = df/du
+ * @param dt
+ * sampling time
+ * @param K
+ * trajectory of resulting control feedback arrays
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving for K
+ */
+ void designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+ //! design a time-varying feedback trajectory using a user-provided derivative-pointer
+ /*!
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param derivatives
+ * shared_ptr to a LinearSystem, allowing to compute A and B
+ * @param dt
+ * sampling time
+ * @param K
+ * trajectory of resulting control feedback arrays
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving for K
+ */
+ void designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> derivatives,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+private:
+ //! compute trajectories of A and B matrices along the given reference trajectory using the user-provided derivative instance
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param N
+ * number of discrete sampling points
+ * @param dt
+ * sampling time dt
+ * @param derivatives
+ * user-provided derivatives
+ * @param A
+ * resulting linear state matrices A
+ * @param B
+ * resulting linear state-input matrices B
+ */
+ void linearizeModel(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ size_t N,
+ SCALAR dt,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& derivatives,
+ state_matrix_array_t& A,
+ control_gain_matrix_array_t& B);
+
+
+ //! solve for the LQR feedback gains
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param A
+ * given linear state matrices A
+ * @param B
+ * given linear state-input matrices B
+ * @param N
+ * number of discrete sampling points
+ * @param dt
+ * sampling time
+ * @param K
+ * the resulting array of state-feedback matrices
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving
+ */
+ void solve(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ size_t N,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+ std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>>
+ costFunction_; //! a quadratic costfunction for solving the optimal control problem
+ DynamicRiccatiEquation<STATE_DIM, CONTROL_DIM, SCALAR> ricattiEq_; //! the Riccati Equations
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp b/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp
new file mode 100644
index 0000000..c4cb757
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp
@@ -0,0 +1,78 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+#include <matlabCppInterface/Engine.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool LQR<STATE_DIM, CONTROL_DIM>::compute(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool RisDiagonal,
+ bool solveRiccatiIteratively)
+{
+ control_matrix_t R_inverse;
+ state_matrix_t P;
+
+ bool success = care_.solve(Q, R, A, B, P, RisDiagonal, R_inverse, solveRiccatiIteratively);
+
+ K = (R_inverse * (B.transpose() * P));
+
+ return success;
+}
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool LQR<STATE_DIM, CONTROL_DIM>::computeMatlab(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool checkControllability)
+{
+ if (!matlabEngine_.isInitialized())
+ matlabEngine_.initialize();
+
+ matlabEngine_.put("Q", Q);
+ matlabEngine_.put("R", R);
+ matlabEngine_.put("A", A);
+ matlabEngine_.put("B", B);
+
+ if (checkControllability)
+ {
+ matlabEngine_.executeCommand("Co=ctrb(A,B);");
+ matlabEngine_.executeCommand("unco=length(A)-rank(Co);");
+ int uncontrollableStates = 0;
+ matlabEngine_.get("unco", uncontrollableStates);
+
+ if (uncontrollableStates > 0)
+ {
+ std::cout << "System is not controllable, # uncontrollable states: " << uncontrollableStates << std::endl;
+ }
+ }
+
+ matlabEngine_.executeCommand("[K,~,~] = lqr(A,B,Q,R);");
+
+ Eigen::MatrixXd Kmatlab;
+ matlabEngine_.get("K", Kmatlab);
+
+ K = Kmatlab;
+
+ return true;
+}
+#endif //USE_MATLAB_CPP_INTERFACE
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/LQR.hpp b/ct_optcon/include/ct/optcon/lqr/LQR.hpp
new file mode 100644
index 0000000..d222aff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/LQR.hpp
@@ -0,0 +1,88 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "riccati/CARE.hpp"
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+#include <matlabCppInterface/Engine.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *
+ * \brief continuous-time infinite-horizon LQR
+ *
+ * Implements continous-time infinite-horizon LQR.
+ * Resulting feedback law will take the form
+ * \f[
+ * u_{fb} = -K \cdot (x - x_{ref})
+ * \f]
+ *
+ * @tparam STATE_DIM
+ * @tparam CONTROL_DIM
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class LQR
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<double, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ //! design the infinite-horizon LQR controller.
+ /*!
+ * @param Q state-weighting matrix
+ * @param R control input weighting matrix
+ * @param A linear system dynamics matrix A
+ * @param B linear system dynamics matrix B
+ * @param K control feedback matrix K (to be designed)
+ * @param RisDiagonal set to true if R is a diagonal matrix (efficiency boost)
+ * @param solveRiccatiIteratively
+ * use closed-form solution of the infinite-horizon Riccati Equation
+ * @return success
+ */
+ bool compute(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool RisDiagonal = false,
+ bool solveRiccatiIteratively = false);
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+ //! design the LQR controller in MATLAB
+ /*!
+ * Note that this controller should be exactly the same
+ */
+ bool computeMatlab(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool checkControllability = false);
+#endif //USE_MATLAB_CPP_INTERFACE
+
+
+private:
+ CARE<STATE_DIM, CONTROL_DIM> care_; // continuous-time algebraic riccati equation
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+ matlab::Engine matlabEngine_;
+#endif
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp
new file mode 100644
index 0000000..9848704
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+CARE<STATE_DIM, CONTROL_DIM>::CARE()
+{
+ // we have to find the optimal work size of schur reordering
+ schur_matrix_t T;
+ schur_matrix_t U;
+
+ int SELECT[2 * STATE_DIM];
+ int N = 2 * STATE_DIM;
+ double WR[T.ColsAtCompileTime];
+ double WI[T.ColsAtCompileTime];
+ int MS;
+ double S;
+ double SEP;
+ double WORKDUMMY[1];
+ int LWORK = -1;
+ int IWORKQUERY[1];
+ int LIWORK = -1;
+ int INFO = 0;
+ int TCols = schur_matrix_t::ColsAtCompileTime;
+
+#ifdef CT_USE_LAPACK
+ dtrsen_("N", "V", &SELECT[0], &TCols, T.data(), &N, U.data(), &N, &WR[0], &WI[0], &MS, &S, &SEP, WORKDUMMY, &LWORK,
+ &IWORKQUERY[0], &LIWORK, &INFO);
+
+ LWORK_ = WORKDUMMY[0] + 32;
+ LIWORK_ = IWORKQUERY[0] + 32;
+
+ WORK_.resize(LWORK_);
+ IWORK_.resize(LIWORK_);
+
+ if (INFO != 0)
+ {
+ std::cout << "Lapack invocation of dtrsen failed!" << std::endl;
+ exit(-1);
+ }
+#endif
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solve(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t& P,
+ bool RisDiagonal,
+ control_matrix_t& R_inverse,
+ bool useIterativeSolver)
+{
+ if (RisDiagonal)
+ {
+ R_inverse.setZero();
+ R_inverse.diagonal().noalias() = R.diagonal().cwiseInverse();
+ }
+ else
+ {
+ R_inverse.noalias() = R.inverse();
+ }
+
+ schur_matrix_t M;
+ M << A, -B * R_inverse * B.transpose(), -Q, -A.transpose();
+
+ if (useIterativeSolver)
+ return solveSchurIterative(M, P);
+ else
+ return solveSchurDirect(M, P);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+typename CARE<STATE_DIM, CONTROL_DIM>::state_matrix_t CARE<STATE_DIM, CONTROL_DIM>::computeSteadyStateRiccatiMatrix(
+ const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ const bool RisDiagonal,
+ const bool useIterativeSolver)
+{
+ state_matrix_t P;
+ control_matrix_t Rinv;
+
+ solve(Q, R, A, B, P, RisDiagonal, Rinv, useIterativeSolver);
+
+ return P;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solveSchurIterative(const schur_matrix_t& M,
+ state_matrix_t& P,
+ double epsilon,
+ int maxIterations)
+{
+ bool converged = false;
+
+ schur_matrix_t Mlocal = M;
+
+ int iterations = 0;
+ while (!converged)
+ {
+ if (iterations > maxIterations)
+ return false;
+
+ schur_matrix_t Mdiff = Mlocal - Mlocal.inverse();
+
+ schur_matrix_t Mnew = Mlocal - 0.5 * Mdiff;
+
+ converged = Mnew.isApprox(Mlocal, epsilon);
+
+ Mlocal = Mnew;
+
+ iterations++;
+ }
+
+ /* break down W and extract W11 W12 W21 W22 (what is the size of these?) */
+ state_matrix_t M11(Mlocal.template block<STATE_DIM, STATE_DIM>(0, 0));
+ state_matrix_t M12(Mlocal.template block<STATE_DIM, STATE_DIM>(0, STATE_DIM));
+ state_matrix_t M21(Mlocal.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0));
+ state_matrix_t M22(Mlocal.template block<STATE_DIM, STATE_DIM>(STATE_DIM, STATE_DIM));
+
+ /* find M and N using the elements of W */
+ factor_matrix_t U;
+ factor_matrix_t V;
+
+ U.template block<STATE_DIM, STATE_DIM>(0, 0) = M12;
+ U.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0) = M22 + state_matrix_t::Identity();
+
+ V.template block<STATE_DIM, STATE_DIM>(0, 0) = M11 + state_matrix_t::Identity();
+ V.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0) = M21;
+
+
+ /* Solve for S from the equation MS=N */
+ FullPivLU_.compute(U);
+
+ P = FullPivLU_.solve(-V);
+
+ return true;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solveSchurDirect(const schur_matrix_t& M, state_matrix_t& P)
+{
+#ifdef CT_USE_LAPACK
+ const bool computeU = true;
+ schur_.compute(M, computeU);
+
+ if (schur_.info() != Eigen::Success)
+ {
+ throw std::runtime_error(
+ "LQR Schur computation failed. Most likely problem is set up wrongly or not solvable.");
+ }
+
+ schur_matrix_t U(schur_.matrixU());
+ schur_matrix_t T(schur_.matrixT());
+
+ int SELECT[2 * STATE_DIM];
+ double WR[2 * STATE_DIM];
+ double WI[2 * STATE_DIM];
+ int MS;
+ double S;
+ double SEP;
+ int INFO = 0;
+ int N = 2 * STATE_DIM;
+
+ for (size_t i = 0; i < 2 * STATE_DIM; i++)
+ {
+ // check if last row or eigenvalue is complex (2x2 block)
+ if (i == (2 * STATE_DIM - 1) || std::abs(T(i + 1, i)) < 1e-12)
+ {
+ SELECT[i] = static_cast<int>(T(i, i) < 0);
+ }
+ else
+ {
+ // we have a complex block
+ SELECT[i] = static_cast<int>((T(i, i) + T(i + 1, i + 1)) / 2.0 < 0);
+ SELECT[i + 1] = SELECT[i];
+ i++;
+ }
+ }
+
+ dtrsen_("N", "V", &SELECT[0], &N, T.data(), &N, U.data(), &N, &WR[0], &WI[0], &MS, &S, &SEP, WORK_.data(), &LWORK_,
+ IWORK_.data(), &LIWORK_, &INFO);
+
+ const state_matrix_t& U11 = U.template block<STATE_DIM, STATE_DIM>(0, 0);
+ const state_matrix_t& U21 = U.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0);
+
+ // solve here for better numerical properties
+ P.noalias() = U21 * U11.inverse();
+
+ if (INFO != 0)
+ {
+ return false;
+ }
+
+ return true;
+#else
+ throw std::runtime_error(
+ "solveSchurDirect() in CARE can only be used if the lapack library is installed on your system.");
+#endif
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp
new file mode 100644
index 0000000..b47f882
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp
@@ -0,0 +1,100 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+
+// Schur reordering from Lapack
+#ifdef CT_USE_LAPACK
+extern "C" void dtrsen_(const char* JOB,
+ const char* COMPQ,
+ const int* SELECT,
+ const int* N,
+ const double* T,
+ const int* LDT,
+ const double* Q,
+ const int* LDQ,
+ double* WR,
+ double* WI,
+ int* M,
+ double* S,
+ double* SEP,
+ double* WORK,
+ const int* LWORK,
+ int* IWORK,
+ const int* LIWORK,
+ int* INFO);
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Continuous-Time Algebraic Riccati Equation
+ *
+ * solves the continuous-time Infinite-Horizon Algebraic Riccati Equation
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class CARE
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<double, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ typedef Eigen::Matrix<double, 2 * STATE_DIM, 2 * STATE_DIM> schur_matrix_t;
+ typedef Eigen::Matrix<double, 2 * STATE_DIM, STATE_DIM> factor_matrix_t;
+
+ CARE();
+
+ // Implementation using the Schur-Method
+ // This is numerically more stable and should be preferred over the naive implementation
+ bool solve(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t& P,
+ bool RisDiagonal,
+ control_matrix_t& R_inverse,
+ bool useIterativeSolver = false);
+
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ const bool RisDiagonal = false,
+ const bool useIterativeSolver = false);
+
+private:
+ bool solveSchurIterative(const schur_matrix_t& M,
+ state_matrix_t& P,
+ double epsilon = 1e-6,
+ int maxIterations = 100000);
+
+ bool solveSchurDirect(const schur_matrix_t& M, state_matrix_t& P);
+
+ Eigen::RealSchur<schur_matrix_t> schur_;
+ Eigen::FullPivLU<factor_matrix_t> FullPivLU_;
+
+ // Lapack
+ int LWORK_;
+ int LIWORK_;
+
+ Eigen::VectorXd WORK_;
+ Eigen::VectorXi IWORK_;
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp
new file mode 100644
index 0000000..30484a4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::DARE()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DARE<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool verbose,
+ const SCALAR eps,
+ size_t maxIter)
+{
+ // If initialized to zero, in the next iteration P becomes Q. Directly initializing to Q avoids the extra iteration.
+ return computeSteadyStateRiccatiMatrix(Q, R, A, B, Q, K, verbose, eps, maxIter);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DARE<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t P,
+ control_feedback_t& K,
+ bool verbose,
+ const SCALAR eps,
+ size_t maxIter)
+{
+ state_matrix_t P_prev;
+ size_t numIter = 0;
+
+ SCALAR diff = 1;
+
+ while (diff >= eps && numIter < maxIter)
+ {
+ P_prev = P;
+ dynamicRDE_.iterateRobust(Q, R, A, B, P, K);
+ diff = (P - P_prev).cwiseAbs().maxCoeff();
+ if (!K.allFinite())
+ throw std::runtime_error("DARE : Failed to converge");
+ numIter++;
+ }
+
+ if (diff >= eps) throw std::runtime_error("DARE : Failed to converge");
+
+ if (verbose)
+ {
+ std::cout << "DARE : converged after " << numIter << " iterations out of a maximum of " << maxIter << std::endl;
+ std::cout << "Resulting K: " << K << std::endl;
+ }
+
+ return P;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp
new file mode 100644
index 0000000..2255eb6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp
@@ -0,0 +1,84 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include "DynamicRiccatiEquation.hpp"
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Discrete-Time Algebraic Riccati Equation
+ *
+ * solves the discrete-time Infinite-Horizon Algebraic Riccati Equation iteratively
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DARE
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ DARE();
+
+ /*! compute the discrete-time steady state Riccati-Matrix
+ * this method iterates over the time-varying discrete-time Riccati Equation to compute the steady-state solution.
+ * @param Q state weight
+ * @param R control weight
+ * @param A discrete-time linear system matrix A
+ * @param B discrete-time linear system matrix B
+ * @param verbose print additional information
+ * @param eps treshold to stop iterating
+ * @return steady state riccati matrix P
+ */
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool verbose = false,
+ const SCALAR eps = 1e-6,
+ size_t maxIter = 1000);
+
+ /*! compute the discrete-time steady state Riccati-Matrix with warm initialization of P matrix
+ * this method iterates over the time-varying discrete-time Riccati Equation to compute the steady-state solution.
+ * @param Q state weight
+ * @param R control weight
+ * @param A discrete-time linear system matrix A
+ * @param B discrete-time linear system matrix B
+ * @param P warm initialized P matrix
+ * @param verbose print additional information
+ * @param eps treshold to stop iterating
+ * @return steady state riccati matrix P
+ */
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t P,
+ control_feedback_t& K,
+ bool verbose = false,
+ const SCALAR eps = 1e-6,
+ size_t maxIter = 1000);
+
+
+private:
+ DynamicRiccatiEquation<STATE_DIM, CONTROL_DIM> dynamicRDE_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp
new file mode 100644
index 0000000..a22c117
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp
@@ -0,0 +1,120 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Dynamic Riccati Equation
+ *
+ * solves the Dynamic Algebraic Riccati Equation
+ *
+ * @tparam STATE_DIM the system state dimension
+ * @tparam CONTROL_DIM the system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DynamicRiccatiEquation
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, 1> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+
+ DynamicRiccatiEquation() : epsilon_(1e-5), eigenvalueSolver_(CONTROL_DIM){};
+
+ ~DynamicRiccatiEquation(){};
+
+ // Naive, direct literal implementation
+ // computes P = Q + A^T ( P - P B ( R + B^T P B )^-1 B^T P) A.
+ // = Q + A^T ( P - P B H_inverse B^T P) A
+ // = Q + A^T S A
+ // where H_inverse = (R + B^T P B)^-1 and S = P - P B H_inverse B^T * P
+ // i.e. P is altered and H_inverse is returned for convenience and use in other functions
+ void iterateNaive(const state_matrix_t& Q_n,
+ const control_matrix_t& R_n,
+ const state_matrix_t& A_n,
+ const control_gain_matrix_t& B_n,
+ state_matrix_t& P_np1,
+ control_feedback_t& K_n)
+ {
+ control_matrix_t H_n = R_n;
+ H_n.noalias() += B_n.transpose() * P_np1 * B_n;
+ control_matrix_t H_n_inverse = H_n.inverse();
+
+ //std::cout << "H_n (naive)" << std::endl << H_n << std::endl;
+ //std::cout << "H_n_inverse (naive)" << std::endl << H_n.inverse() << std::endl;
+
+ K_n.noalias() = -1.0 * H_n_inverse * B_n.transpose() * P_np1 * A_n;
+
+ // here we compute P_n
+ P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
+ P_np1.noalias() -= K_n.transpose() * H_n * K_n;
+
+ P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
+ }
+
+
+ void iterateRobust(const state_matrix_t& Q_n,
+ const control_matrix_t& R_n,
+ const state_matrix_t& A_n,
+ const control_gain_matrix_t& B_n,
+ state_matrix_t& P_np1,
+ control_feedback_t& K_n)
+ {
+ control_matrix_t H_n = R_n;
+ H_n.noalias() += B_n.transpose() * P_np1 * B_n;
+ H_n = (H_n + H_n.transpose()).eval() / 2.0;
+
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(H_n, true);
+ const control_matrix_t& V = eigenvalueSolver_.eigenvectors().real();
+ const control_vector_t& lambda = eigenvalueSolver_.eigenvalues().real();
+ assert(eigenvalueSolver_.eigenvectors().imag().norm() < 1e-7 && "Eigenvectors not real");
+ assert(eigenvalueSolver_.eigenvalues().imag().norm() < 1e-7 && "Eigenvalues is not real");
+ // Corrected Eigenvalue Matrix
+ control_matrix_t D = control_matrix_t::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(control_vector_t::Zero()) + epsilon_ * control_vector_t::Ones();
+
+ assert((V.transpose() - V.inverse()).norm() < 1e-5 && "WARNING: V transpose and V inverse are not similar!");
+
+ // reconstruct H
+ H_n.noalias() = V * D * V.transpose();
+
+ // invert D
+ control_matrix_t D_inverse = control_matrix_t::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = D.diagonal().cwiseInverse();
+ control_matrix_t H_n_inverse = V * D_inverse * V.transpose();
+
+ K_n.noalias() = -1.0 * H_n_inverse * (B_n.transpose() * P_np1 * A_n);
+
+ // here we compute P_n
+ P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
+ P_np1.noalias() -= K_n.transpose() * H_n * K_n;
+
+ P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
+ }
+
+
+private:
+ SCALAR epsilon_;
+ Eigen::EigenSolver<control_matrix_t> eigenvalueSolver_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/matlab.hpp b/ct_optcon/include/ct/optcon/matlab.hpp
new file mode 100644
index 0000000..7f3fe1c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/matlab.hpp
@@ -0,0 +1,24 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef MATLAB_FULL_LOG
+#define MATLAB
+#endif // MATLAB_FULL_LOG
+
+#ifdef MATLAB
+#include <matlabCppInterface/MatFile.hpp>
+#else //MATLAB
+namespace matlab {
+//! a dummy class which is created for compatibility reasons if the MATLAB flag is not set.
+class MatFile
+{
+public:
+ MatFile() {}
+};
+} // namespace matlab
+#endif //MATLAB
diff --git a/ct_optcon/include/ct/optcon/mpc/MPC-impl.h b/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
new file mode 100644
index 0000000..81adecd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
@@ -0,0 +1,350 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename OPTCON_SOLVER>
+MPC<OPTCON_SOLVER>::MPC(const OptConProblem_t& problem,
+ const typename OPTCON_SOLVER::Settings_t& solverSettings,
+ const mpc_settings& mpcsettings,
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> customPolicyHandler,
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> customTimeHorizon)
+ :
+
+ solver_(problem, solverSettings),
+ mpc_settings_(mpcsettings),
+ dynamics_(problem.getNonlinearSystem()->clone()),
+ forwardIntegrator_(dynamics_, mpcsettings.stateForwardIntegratorType_),
+ firstRun_(true),
+ runCallCounter_(0),
+ policyHandler_(new PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>())
+{
+ checkSettings(mpcsettings);
+
+ // =========== INIT WARM START STRATEGY =============
+
+ if (mpc_settings_.coldStart_ == false)
+ {
+ if (customPolicyHandler)
+ {
+ std::cout << "Initializing MPC with a custom policy handler (warmstarter) provided by the user."
+ << std::endl;
+ policyHandler_ = customPolicyHandler;
+ }
+ else
+ {
+ if (std::is_base_of<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, Scalar_t>, OPTCON_SOLVER>::value)
+ {
+ // default policy handler for standard discrete-time iLQG implementation
+ policyHandler_ = std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>>(
+ new StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, Scalar_t>(solverSettings.dt));
+ }
+ else
+ {
+ throw std::runtime_error(
+ "ERROR in MPC Constructor -- no default warm start strategy available for the selected "
+ "controller.");
+ }
+ }
+ }
+
+
+ // ============== INIT MPC TIME HORIZON STRATEGY ==============
+ if (customTimeHorizon)
+ {
+ std::cout << "Initializing MPC with a custom time-horizon strategy provided by the user." << std::endl;
+ timeHorizonStrategy_ = customTimeHorizon;
+ }
+ else
+ {
+ const Scalar_t initTimeHorizon = solver_.getTimeHorizon();
+
+ timeHorizonStrategy_ = std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>>(
+ new tpl::MpcTimeHorizon<Scalar_t>(mpc_settings_, initTimeHorizon));
+ }
+
+
+ // ============== INIT MPC TIME KEEPER ==============
+ timeKeeper_ = tpl::MpcTimeKeeper<Scalar_t>(timeHorizonStrategy_, mpc_settings_);
+}
+
+
+template <typename OPTCON_SOLVER>
+OPTCON_SOLVER& MPC<OPTCON_SOLVER>::getSolver()
+{
+ return solver_;
+};
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::setTimeHorizonStrategy(std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy)
+{
+ timeHorizonStrategy_ = timeHorizonStrategy;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::setInitialGuess(const Policy_t& initGuess)
+{
+ solver_.setInitialGuess(initGuess);
+ policyHandler_->setPolicy(initGuess);
+ currentPolicy_ = initGuess;
+}
+
+
+template <typename OPTCON_SOLVER>
+bool MPC<OPTCON_SOLVER>::timeHorizonReached()
+{
+ return timeKeeper_.finalPointReached();
+}
+
+
+template <typename OPTCON_SOLVER>
+const typename MPC<OPTCON_SOLVER>::Scalar_t MPC<OPTCON_SOLVER>::timeSinceFirstSuccessfulSolve(const Scalar_t& extTime)
+{
+ return timeKeeper_.timeSinceFirstSuccessfulSolve(extTime);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::doForwardIntegration(const Scalar_t& t_forward_start,
+ const Scalar_t& t_forward_stop,
+ core::StateVector<STATE_DIM, Scalar_t>& x_start,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController)
+{
+ if (mpc_settings_.stateForwardIntegration_ == true)
+ {
+ if (forwardIntegrationController)
+ {
+ // ... either with a given third-party controller
+ integrateForward(t_forward_start, t_forward_stop, x_start, forwardIntegrationController);
+ }
+ else
+ {
+ // ... or with the controller obtained from the solver (solution of last mpc-run).
+ std::shared_ptr<Policy_t> prevController(new Policy_t(currentPolicy_));
+ integrateForward(t_forward_start, t_forward_stop, x_start, prevController);
+ }
+ }
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::prepareIteration(const Scalar_t& extTime)
+{
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: started to prepare MPC iteration() " << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ runCallCounter_++;
+
+ const Scalar_t currTimeHorizon = solver_.getTimeHorizon();
+ Scalar_t newTimeHorizon;
+
+
+ if (firstRun_)
+ timeKeeper_.initialize();
+
+ timeKeeper_.computeNewTimings(extTime, currTimeHorizon, newTimeHorizon, t_forward_start_, t_forward_stop_);
+
+ // update the Optimal Control Solver with new time horizon and state information
+ solver_.changeTimeHorizon(newTimeHorizon);
+
+
+ // Calculate new initial guess / warm-starting policy
+ policyHandler_->designWarmStartingPolicy(t_forward_stop_, newTimeHorizon, currentPolicy_);
+
+ // todo: remove this after through testing
+ if (t_forward_stop_ < t_forward_start_)
+ throw std::runtime_error("ERROR: t_forward_stop < t_forward_start is impossible.");
+
+
+ /**
+ * re-initialize the OptConSolver and solve the optimal control problem.
+ */
+ solver_.setInitialGuess(currentPolicy_);
+
+ solver_.prepareMPCIteration();
+}
+
+
+template <typename OPTCON_SOLVER>
+bool MPC<OPTCON_SOLVER>::finishIteration(const core::StateVector<STATE_DIM, Scalar_t>& x,
+ const Scalar_t x_ts,
+ Policy_t& newPolicy,
+ Scalar_t& newPolicy_ts,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController)
+{
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: started mpc finish Iteration() with state-timestamp " << x_ts << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ timeKeeper_.startDelayMeasurement(x_ts);
+
+ // initialize the time-stamp for policy which is to be designed
+ newPolicy_ts = x_ts;
+
+ core::StateVector<STATE_DIM, Scalar_t> x_start = x;
+
+ if (!firstRun_)
+ doForwardIntegration(t_forward_start_, t_forward_stop_, x_start, forwardIntegrationController);
+
+
+ solver_.changeInitialState(x_start);
+
+ bool solveSuccessful = solver_.finishMPCIteration();
+
+ if (solveSuccessful)
+ {
+ newPolicy_ts = newPolicy_ts + (t_forward_stop_ - t_forward_start_);
+
+ // get optimized policy and state trajectory from OptConSolver
+ currentPolicy_ = solver_.getSolution();
+
+ // obtain the time which passed since the previous successful solve
+ Scalar_t dtp = timeKeeper_.timeSincePreviousSuccessfulSolve(x_ts);
+
+ // post-truncation may be an option of the solve-call took longer than the estimated delay
+ if (mpc_settings_.postTruncation_)
+ {
+ if (dtp > t_forward_stop_ && !firstRun_)
+ {
+ // the time-difference to be account for by post-truncation
+ Scalar_t dt_post_truncation = dtp - t_forward_stop_;
+
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: additional post-truncation about " << dt_post_truncation << " [sec]."
+ << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ // the time which was effectively truncated away (e.g. discrete-time case)
+ Scalar_t dt_truncated_eff;
+
+ policyHandler_->truncateSolutionFront(dt_post_truncation, currentPolicy_, dt_truncated_eff);
+
+ // update policy timestamp with the truncated time
+ newPolicy_ts += dt_truncated_eff;
+ }
+ else if (t_forward_stop_ >= dtp && !firstRun_)
+ {
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: controller opt faster than pre-integration horizon. Consider tuning "
+ "pre-integration. "
+ << std::endl;
+#endif //DEBUG_PRINT_MPC
+ }
+
+ } // post truncation
+
+ } // solve successful
+
+
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: start timestamp outgoing policy: " << newPolicy_ts << std::endl;
+ std::cout << "DEBUG_PRINT_MPC: ended finishIteration() " << std::endl << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ // update policy result
+ newPolicy = currentPolicy_;
+
+ // stop the delay measurement. This needs to be the last method called in finishIteration().
+ timeKeeper_.stopDelayMeasurement(x_ts);
+
+ // in the first run, the policy time-stamp needs to be shifted about the solving time
+ if (firstRun_)
+ {
+ newPolicy_ts = newPolicy_ts + timeKeeper_.getMeasuredDelay();
+ firstRun_ = false;
+ }
+
+ return solveSuccessful;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::resetMpc(const Scalar_t& newTimeHorizon)
+{
+ firstRun_ = true;
+
+ runCallCounter_ = 0;
+
+ // reset the time horizon of the strategy
+ timeHorizonStrategy_->updateInitialTimeHorizon(newTimeHorizon);
+
+ solver_.changeTimeHorizon(newTimeHorizon);
+
+ timeKeeper_.initialize();
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::updateSettings(const mpc_settings& settings)
+{
+ checkSettings(settings);
+
+ if (settings.stateForwardIntegratorType_ != mpc_settings_.stateForwardIntegratorType_)
+ forwardIntegrator_ = ct::core::Integrator<STATE_DIM, Scalar_t>(dynamics_, settings.stateForwardIntegratorType_);
+
+ mpc_settings_ = settings;
+ timeKeeper_.updateSettings(settings);
+ timeHorizonStrategy_->updateSettings(settings);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::printMpcSummary()
+{
+ std::cout << std::endl;
+ std::cout << "================ MPC Summary ================" << std::endl;
+ std::cout << "Number of MPC calls:\t\t\t" << runCallCounter_ << std::endl;
+
+ if (mpc_settings_.measureDelay_)
+ {
+ std::cout << "Max measured solving time [sec]:\t" << timeKeeper_.getMaxMeasuredDelay() << std::endl;
+ std::cout << "Min measured solving time [sec]:\t" << timeKeeper_.getMinMeasuredDelay() << std::endl;
+ std::cout << "Total sum of meas. delay [sec]: \t" << timeKeeper_.getSummedDelay() << std::endl;
+ std::cout << "Average measured delay [sec]: \t" << timeKeeper_.getSummedDelay() / runCallCounter_
+ << std::endl;
+ }
+ else
+ {
+ std::cout << "Used fixed delay[sec]: \t" << 0.000001 * mpc_settings_.fixedDelayUs_ << std::endl;
+ }
+
+ std::cout << "================ End Summary ================" << std::endl;
+ std::cout << std::endl;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::integrateForward(const Scalar_t startTime,
+ const Scalar_t stopTime,
+ core::StateVector<STATE_DIM, Scalar_t>& state,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>>& controller)
+{
+ dynamics_->setController(controller);
+
+ int nSteps = std::max(0, (int)std::lround((stopTime - startTime) / mpc_settings_.stateForwardIntegration_dt_));
+
+ // adaptive pre-integration
+ forwardIntegrator_.integrate_n_steps(state, startTime, nSteps, mpc_settings_.stateForwardIntegration_dt_);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::checkSettings(const mpc_settings& settings)
+{
+ // if external timing is active, internal delay measurement must be turned off
+ if (settings.useExternalTiming_ && settings.measureDelay_)
+ throw std::runtime_error(
+ "MPC: measuring delay internally contradicts using external Timing. Switch off one of those options.");
+}
+
+} //namespace optcon
+} //namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MPC.h b/ct_optcon/include/ct/optcon/mpc/MPC.h
new file mode 100644
index 0000000..8a84461
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MPC.h
@@ -0,0 +1,256 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <type_traits>
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include "MpcSettings.h"
+#include "MpcTimeKeeper.h"
+
+#include "policyhandler/PolicyHandler.h"
+#include "timehorizon/MpcTimeHorizon.h"
+
+#include <ct/optcon/solver/NLOptConSolver.hpp>
+#include "policyhandler/default/StateFeedbackPolicyHandler.h"
+
+//#define DEBUG_PRINT_MPC //! use this flag to enable debug printouts in the MPC implementation
+
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup MPC MPC
+ *
+ * \brief Model Predictive Control Module
+ */
+
+/**
+ * \ingroup MPC
+ *+
+ * \brief Main MPC class.
+ *
+ * This MPC class allows to use any solver that derives from the OptConSolver base class in Model-Predictive-Control fashion.
+ * MPC will automatically construct the solver
+ *
+ * Main assumptions:
+ * This MPC class is deliberately designed such that the time-keeping is managed by itself. The main assumption is that the controller
+ * which is designed here, gets applied to the system instantaneously after the run() call is executed. Furthermore, we assume that all Optimal Control Problems start at
+ * time zero. This also applies to the cost- and the constraint functionals which are to be provided by the user.
+ *
+ * Sidenotes:
+ * between the calls to run(), the user can arbitrarily modify his cost-functions, etc. in order to change the problem.
+ *
+ * @param OPTCON_SOLVER
+ * the optimal control solver to be employed, for example SLQ or DMS
+ *
+ */
+template <typename OPTCON_SOLVER>
+class MPC
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_DIM = OPTCON_SOLVER::STATE_D;
+ static const size_t CONTROL_DIM = OPTCON_SOLVER::CONTROL_D;
+
+ static const size_t P_DIM = OPTCON_SOLVER::POS_DIM;
+ static const size_t V_DIM = OPTCON_SOLVER::VEL_DIM;
+
+ typedef typename OPTCON_SOLVER::Scalar_t Scalar_t;
+ typedef typename OPTCON_SOLVER::Policy_t Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, Scalar_t> OptConProblem_t;
+
+
+ //! MPC solver constructor
+ /*!
+ *
+ * @param problem
+ * the optimal control problem set up by the user
+ * @param solverSettings
+ * settings class/struct for the optimal control solver of choice. Be sure to tune the solver settings such that they are suitable for MPC!
+ * @param mpcsettings
+ * mpc-specific settings, see class MpcSettings.h
+ * @param customPolicyHandler
+ * user-provided custom policy handler, which derives from base class 'PolicyHandler'.
+ * If not specified, MPC will use one of its default implementations or throw an error if there is no default match.
+ * @param customTimeHorizon
+ * user-provided custom time horizon strategy, which derives from base class 'MpcTimeHorizon'.
+ * If not specified, MPC will use one of its default implementations or throw an error if there is no default match.
+ */
+ MPC(const OptConProblem_t& problem,
+ const typename OPTCON_SOLVER::Settings_t& solverSettings,
+ const mpc_settings& mpcsettings = mpc_settings(),
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> customPolicyHandler = nullptr,
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> customTimeHorizon = nullptr);
+
+
+ //! Allows access to the solver member, required mainly for unit testing.
+ /*!
+ * @return reference to the optimal control problem solver
+ */
+ OPTCON_SOLVER& getSolver();
+
+
+ //! Additional method to insert a custom time horizon strategy, independent from the constructor
+ /*!
+ * @param timeHorizonStrategy
+ * the time horizon strategy provided by the user
+ */
+ void setTimeHorizonStrategy(std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy);
+
+ //! set a new initial guess for the policy
+ /**
+ * @param initGuess
+ */
+ void setInitialGuess(const Policy_t& initGuess);
+
+ //! Check if final time horizon for this task was reached
+ bool timeHorizonReached();
+
+
+ //! retrieve the time that elapsed since the first successful solve() call to an Optimal Control Problem
+ /*!
+ * @param the external time stamp
+ * @return time elapsed, the returned time can be used externally, for example to update cost functions
+ */
+ const Scalar_t timeSinceFirstSuccessfulSolve(const Scalar_t& extTime);
+
+
+ //! perform forward integration of the measured system state, to compensate for expected or already occurred time lags
+ /*!
+ * State forward integration
+ * @param t_forward_start
+ * time where forward integration starts
+ * @param t_forward_stop
+ * time where forward integration stops
+ * @param x_start
+ * initial state for forward integration, gets overwritten with forward-integrated state
+ * @param forwardIntegrationController
+ * (optional) external controller for forward integration
+ *
+ * \warning The effect of the integration will vanish one the MPC frequency is higher than the sampling frequency
+ */
+ void doForwardIntegration(const Scalar_t& t_forward_start,
+ const Scalar_t& t_forward_stop,
+ core::StateVector<STATE_DIM, Scalar_t>& x_start,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
+ nullptr);
+
+
+ /*!
+ * Prepare MPC iteration
+ * @param ext_ts the current external time
+ */
+ void prepareIteration(const Scalar_t& ext_ts);
+
+
+ //! finish MPC iteration
+ /*!
+ * @param x
+ * current system state
+ * @param x_ts
+ * time stamp of the current state (external time in seconds)
+ * @param newPolicy
+ * the new policy calculated based on above state, the timing info and the underlying OptConProblem
+ * @param newPolicy_ts
+ * time stamp of the resulting policy. This indicates when the policy is supposed to start being applied, relative to
+ * the user-provided state-timestamp x_ts.
+ * @param forwardIntegrationController
+ * optional input: in some scenarios, we wish to use a different kind controller for forward integrating the system than the one we are optimizing
+ * Such a controller can be handed over here as additional argument. If set to empty, MPC uses its own optimized controller from
+ * the last iteration, thus assuming perfect control trajectory tracking.
+ * @return true if solve was successful, false otherwise.
+ */
+ bool finishIteration(const core::StateVector<STATE_DIM, Scalar_t>& x,
+ const Scalar_t x_ts,
+ Policy_t& newPolicy,
+ Scalar_t& newPolicy_ts,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
+ nullptr);
+
+
+ //! reset the mpc problem and provide new problem time horizon (mandatory)
+ void resetMpc(const Scalar_t& newTimeHorizon);
+
+
+ //! update the mpc settings in all instances (main class, time keeper class, etc)
+ /*!
+ * update the mpc settings in all instances
+ * @param settings
+ * the new mpc settings provided by the user.
+ */
+ void updateSettings(const mpc_settings& settings);
+
+
+ //! printout simple statistical data
+ void printMpcSummary();
+
+
+private:
+ //! state forward propagation (for delay compensation)
+ /*!
+ * Perform forward integration about the given prediction horizon.
+ * - uses an arbitrary controller given, which is important for hierarchical setups where the actual controller may be refined further
+ * @param startTime
+ * time where forward integration starts w.r.t. the current policy
+ * @param stopTime
+ * time where forward integration stops w.r.t. the current policy
+ * @param state
+ * state to be forward propagated
+ * @param controller
+ * the controller to be used for forward propagation
+ */
+ void integrateForward(const Scalar_t startTime,
+ const Scalar_t stopTime,
+ core::StateVector<STATE_DIM, Scalar_t>& state,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>>& controller);
+
+ void checkSettings(const mpc_settings& settings);
+
+ //! timings for pre-integration
+ Scalar_t t_forward_start_;
+ Scalar_t t_forward_stop_;
+
+ //! optimal control solver employed for mpc
+ OPTCON_SOLVER solver_;
+
+ //! currently optimal policy, initial guess respectively
+ Policy_t currentPolicy_;
+
+ //! policy handler, which takes care of warm-starting
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> policyHandler_;
+
+ //! time horizon strategy, e.g. receding horizon optimal control
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy_;
+
+ //! time keeper
+ tpl::MpcTimeKeeper<Scalar_t> timeKeeper_;
+
+ //! mpc settings
+ mpc_settings mpc_settings_;
+
+ //! true for first run
+ bool firstRun_;
+
+ //! dynamics instance for forward integration
+ typename OPTCON_SOLVER::OptConProblem_t::DynamicsPtr_t dynamics_;
+
+ //! integrator for forward integration
+ ct::core::Integrator<STATE_DIM, Scalar_t> forwardIntegrator_;
+
+ //! counter which gets incremented at every call of the run() method
+ size_t runCallCounter_;
+};
+
+
+} //namespace optcon
+} //namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MpcSettings.h b/ct_optcon/include/ct/optcon/mpc/MpcSettings.h
new file mode 100644
index 0000000..2c9b97b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MpcSettings.h
@@ -0,0 +1,172 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+//! select a mode in which MPC is supposed to run
+/*!
+ * There are four default implementations for time horizon strategies in MPC, which can be selected using the following "MPC_MODE" enum.
+ *
+ * - FIXED_FINAL_TIME
+ * plan until a fixed final time T is reached. Time Horizon will continuously shrink and eventually be zero.
+ *
+ * - FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON
+ * this option continuously shrinks the time horizon from the initil time horizon until some minimum time is reached.
+ * This minimum time can be specified in the mpc_settings struct as "minimumTimeHorizonMpc_"
+ *
+ * - CONSTANT_RECEDING_HORIZON
+ * Time Horizon remains constant for all times and is equal to the initial time horizon specified in the optimal control problem.
+ *
+ * - RECEDING_HORIZON_WITH_FIXED_FINAL_TIME
+ * Time Horizon remains constant until a fixed final time is near. It shrinks and eventually becomes zero.
+ * The overall time horizon gets set trough the initial problem time horizon. The smaller, receding time
+ * horizon can be specified in the mpc_settings struct as "minimumTimeHorizonMpc_"
+ */
+enum MPC_MODE
+{
+
+ FIXED_FINAL_TIME = 0, //!< FIXED_FINAL_TIME
+
+ CONSTANT_RECEDING_HORIZON, //!< CONSTANT_RECEDING_HORIZON
+
+ FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON, //!< FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON
+
+ RECEDING_HORIZON_WITH_FIXED_FINAL_TIME //!< RECEDING_HORIZON_WITH_FIXED_FINAL_TIME
+};
+
+
+//! MPC Settings struct
+struct mpc_settings
+{
+ /*!
+ * State prediction.
+ * Use state prediction (based on a given delay, the given initial state and a given policy)?
+ * If set to true, MPC will either use a measurement of the delay or use the fixed delay given below
+ * as "fixedDelayUs_".
+ */
+ bool stateForwardIntegration_ = false;
+
+ /*!
+ * Integrator type to use for the state prediction
+ */
+ ct::core::IntegrationType stateForwardIntegratorType_ = ct::core::IntegrationType::RK4;
+
+ /*!
+ * time step size employed for the forward integration
+ */
+ double stateForwardIntegration_dt_ = 0.001;
+
+ /*!
+ * Delay compensation.
+ * If measureDelay_ is set to true, the MPC timer automatically keeps track of the planning times and
+ * uses them for the state forward prediction.
+ * Only applies if stateForwardIntegration_ is set to true.
+ * */
+ bool measureDelay_ = false;
+
+ /*!
+ * The delayMeasurementMultiplier_ quantifies the level of trust you have in our planning time measurement.
+ * In case you wish to be conservative, for example because the planning times vary a lot, reduce this multiplier.
+ * Maximum confidence means, set it to 1.0.
+ * In essence, this number says "how much of our measured delay do we use for pre-integration".
+ * */
+ double delayMeasurementMultiplier_ = 0.7;
+
+ /*!
+ * If not using measureDelay_ = true, we can instead set a fixed, known delay in microseconds (!) here.
+ */
+ int fixedDelayUs_ = 0;
+
+ /*!
+ * Additional delay in microseconds (!), which gets added to either the measured delay of above fixedDelay.
+ */
+ int additionalDelayUs_ = 0;
+
+ /*!
+ * If solving the problem takes longer than predicted, we can post-truncate the solution trajectories.
+ */
+ bool postTruncation_ = false;
+
+ /*!
+ * MPC Time horizon modus
+ */
+ MPC_MODE mpc_mode = CONSTANT_RECEDING_HORIZON;
+
+ /*!
+ * see description of the different modes above
+ */
+ core::Time minimumTimeHorizonMpc_ = 1.0; // seconds
+
+ /*!
+ * cold starting or warm starting.
+ * Warm starting will either use a default strategy for common controller types, or a user specified
+ * custom warm starting strategy.
+ */
+ bool coldStart_ = false;
+
+
+ /*!
+ * override the internal clock with external timing.
+ * \warning when employing this option, the setExternalTime() method needs to be called in MPC
+ */
+ bool useExternalTiming_ = false;
+
+
+ //! Print MPC settings to console
+ void print()
+ {
+ std::cout << " ========================= MPC SETTINGS =============================" << std::endl;
+ std::cout << " stateForwardIntegration: \t " << stateForwardIntegration_ << std::endl;
+ std::cout << " stateForwardIntegrator: \t " << stateForwardIntegratorType_ << std::endl;
+ std::cout << " stateForwardIntegration_dt: \t " << stateForwardIntegration_dt_ << std::endl;
+ std::cout << " measureDelay: \t " << measureDelay_ << std::endl;
+ std::cout << " delayMeasurementMultiplier: \t " << delayMeasurementMultiplier_ << std::endl;
+ std::cout << " fixedDelayUs: \t " << fixedDelayUs_ << std::endl;
+ std::cout << " additionalDelayUs: \t " << additionalDelayUs_ << std::endl;
+ std::cout << " postTruncation_: \t " << postTruncation_ << std::endl;
+ std::cout << " mpc_mode: \t " << (int)mpc_mode << std::endl;
+ std::cout << " minimumTimeHorizonMpc: \t " << minimumTimeHorizonMpc_ << std::endl;
+ std::cout << " coldStart: \t " << coldStart_ << std::endl;
+ std::cout << " useExternalTiming: \t " << useExternalTiming_ << std::endl;
+ std::cout << " ============================== END =================================" << std::endl;
+ }
+};
+
+
+//! load the mpc settings from file
+/*!
+ * @param filename
+ * path of the file you wish to load from
+ * @param settings
+ * the loaded settings
+ *
+ * @todo make part of mpc_settings struct
+ */
+inline void loadMpcSettings(const std::string& filename, mpc_settings& settings)
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ settings.measureDelay_ = pt.get<bool>("mpc.measureDelay");
+ settings.stateForwardIntegration_ = pt.get<bool>("mpc.stateForwardIntegration");
+ settings.stateForwardIntegration_dt_ = pt.get<double>("mpc.stateForwardIntegration_dt");
+ settings.stateForwardIntegratorType_ = (ct::core::IntegrationType)pt.get<int>("mpc.stateForwardIntegratorType");
+ settings.fixedDelayUs_ = pt.get<int>("mpc.fixedDelayUs");
+ settings.additionalDelayUs_ = pt.get<int>("mpc.additionalDelayUs");
+ settings.minimumTimeHorizonMpc_ = pt.get<double>("mpc.timeHorizon");
+ settings.coldStart_ = pt.get<bool>("mpc.coldStart");
+ settings.postTruncation_ = pt.get<bool>("mpc.postTruncation");
+ settings.delayMeasurementMultiplier_ = pt.get<double>("mpc.delayMeasurementMultiplier");
+ settings.useExternalTiming_ = pt.get<bool>("mpc.useExternalTiming");
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h b/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h
new file mode 100644
index 0000000..0a039ad
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h
@@ -0,0 +1,382 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "MpcSettings.h"
+#include "timehorizon/MpcTimeHorizon.h"
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+/**
+ * define the following flag for debugging the MPC time keeper
+ */
+//#define DEBUG_PRINT_TIMEKEEPER
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+//! Time Keeper Class for Model Predictive Control
+/*!
+ * - uses an internal clock for bookkeeping of times between MPC clycles
+ * - this class is based on the assumption that all Optimal Control Problems start at t = 0. All times computed here are
+ * expressed as relative times w.r.t. the start of the Optimal Control Problem.
+ * - calculates the required shifting times between iterations and due to forward integration (initial state prediction)
+ * - update Time Horizons accordingly
+ */
+template <typename SCALAR = double>
+class MpcTimeKeeper
+{
+public:
+ //! Standard Constructor MpcTimeKeeper
+ /*!
+ * this constructor should not be used by the normal user. Todo: get rid of MpcTimeKeeper default constructor
+ */
+ MpcTimeKeeper() {}
+ //! Constructor for Mpc Time Keeper class
+ /*!
+ *
+ * @param timeHorizonStrategy the user-specified time horizon strategy, for example fixed time horizon
+ * @param mpc_settings the mpc_settings as specified by the user
+ */
+ MpcTimeKeeper(std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy, const mpc_settings& mpc_settings)
+ : mpc_settings_(mpc_settings),
+ initialized_(false),
+ finalPointReached_(false),
+ lastMeasuredDelay_(0.0),
+ maxDelayMeasured_(0.0),
+ minDelayMeasured_(std::numeric_limits<SCALAR>::max()),
+ summedDelay_(0.0),
+ timeHorizonStrategy_(timeHorizonStrategy),
+ firstRun_(true)
+ {
+ }
+
+
+ //! initialize the Mpc Time Keeper (mandatory)
+ /*!
+ * resets the TimeKeeper, needs to be called whenever MPC starts over from the beginning.
+ */
+ void initialize()
+ {
+ firstRun_ = true;
+
+ timer_.reset();
+ lastSolveTimer_.reset();
+ firstSolveTimer_.reset();
+
+ ext_timer_.reset();
+ ext_lastSolveTimer_.reset();
+ ext_firstSolveTimer_.reset();
+
+ lastMeasuredDelay_ = 0.0;
+ maxDelayMeasured_ = 0.0;
+ minDelayMeasured_ = std::numeric_limits<SCALAR>::max();
+ summedDelay_ = 0.0;
+ finalPointReached_ = false;
+
+ initialized_ = true;
+ }
+
+
+ //! compute new mpc timings, based on current time horizon and the given time horizon strategy
+ /*!
+ * @param externalTime the external timing, optional.
+ * @param current_T
+ * the currently active problem time horizon
+ * @param new_T
+ * the new, updated problem time horizon, which gets computed by the time horizon strategy
+ * @param t_forw_start
+ * time where to start the forward propagation of the state measurement based on delays
+ * @param t_forw_stop
+ * time where to stop the forward propagation of the state measurement based on delays
+ */
+ void computeNewTimings(const SCALAR externalTime,
+ const SCALAR current_T,
+ SCALAR& new_T,
+ SCALAR& t_forw_start,
+ SCALAR& t_forw_stop)
+ {
+ if (initialized_ == false)
+ throw std::runtime_error(
+ "Error in MPC time keeper: cannot update timings if MpcTimeKeeper not properly initialized.");
+
+
+ SCALAR timeSinceEndedLastSolve;
+ SCALAR timeSinceEndedFirstSolve;
+
+ if (!firstRun_)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.stop(externalTime);
+ ext_lastSolveTimer_.stop(externalTime);
+ timeSinceEndedFirstSolve = ext_firstSolveTimer_.getElapsedTime();
+ timeSinceEndedLastSolve = ext_lastSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ lastSolveTimer_.stop();
+ firstSolveTimer_.stop();
+ timeSinceEndedLastSolve = lastSolveTimer_.getElapsedTime();
+ timeSinceEndedFirstSolve = firstSolveTimer_.getElapsedTime();
+ }
+ }
+ else
+ {
+ // set zero for the first mpc run
+ timeSinceEndedLastSolve = 0.0;
+ timeSinceEndedFirstSolve = 0.0;
+ }
+
+
+ // starting time of forward prediction, relative to previous controller
+ t_forw_start = timeSinceEndedLastSolve;
+
+
+ /**
+ * estimate the delay from planning, etc.
+ */
+ SCALAR delayToApply = computeDelayToApply();
+
+
+ // stopping time relative to previous controller/run
+ if (!firstRun_)
+ t_forw_stop = t_forw_start + delayToApply;
+ else
+ t_forw_stop = t_forw_start;
+
+
+ /**
+ * check for compliance of t_forward_stop and t_forward_start with time horizon of the current controller
+ */
+ if (t_forw_start > current_T)
+ {
+ std::cerr << "WARNING: forward integration start time cannot be bigger than last time horizon. Truncating "
+ "forward integration time."
+ << std::endl;
+ t_forw_start = current_T;
+ }
+ if (t_forw_stop > current_T)
+ {
+ std::cerr << "WARNING: forward integration stop time cannot be bigger than last time horizon. Truncating "
+ "forward integration time."
+ << std::endl;
+ t_forw_stop = current_T;
+ }
+
+
+ finalPointReached_ = timeHorizonStrategy_->computeNewTimeHorizon(timeSinceEndedFirstSolve, t_forw_stop, new_T);
+
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since first solve(): " << timeSinceEndedFirstSolve << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since last solve(): " << timeSinceEndedLastSolve << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_start: " << t_forw_start << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_stop: " << t_forw_stop << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: New Time Horizon: " << new_T << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: final point reached_: " << finalPointReached_ << std::endl;
+#endif
+ }
+
+
+ //! query this in order to find out if the final time horizon has been reached.
+ /*!
+ * May be used to trigger corresponding events, such as stopping the control loop.
+ * @return bool, true if final time horizon has been reached
+ */
+ const bool finalPointReached() const { return finalPointReached_; }
+ //! update mpc settings
+ /*!
+ * @param settings
+ * the new settings to be handed over
+ */
+ void updateSettings(const mpc_settings& settings) { mpc_settings_ = settings; }
+ //! start measuring time elapsed during planning / solving the optimal control problem
+ void startDelayMeasurement(const SCALAR& externalTime)
+ {
+ if (mpc_settings_.measureDelay_)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ ext_timer_.start(externalTime);
+ else
+ timer_.start();
+ }
+ }
+
+
+ //! stop measuring time elapsed during solving the optimal control problem
+ void stopDelayMeasurement(const SCALAR& externalTime)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_timer_.stop(externalTime);
+ lastMeasuredDelay_ = ext_timer_.getElapsedTime();
+ // measure how much time passed since last successful solve()
+ ext_lastSolveTimer_.start(externalTime);
+ }
+ else
+ {
+ timer_.stop();
+ lastMeasuredDelay_ = timer_.getElapsedTime();
+ // measure how much time passed since last successful solve()
+ lastSolveTimer_.start();
+ }
+
+ maxDelayMeasured_ = std::max(maxDelayMeasured_, lastMeasuredDelay_);
+ minDelayMeasured_ = std::min(minDelayMeasured_, lastMeasuredDelay_);
+ summedDelay_ += lastMeasuredDelay_;
+
+ if (lastMeasuredDelay_ < 0)
+ throw std::runtime_error("Fatal: measured delay cannot be < 0");
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "Measured delay during Solution: " << lastMeasuredDelay_ << " seconds" << std::endl;
+ std::cout << "Max. measured delay during Solution: " << maxDelayMeasured_ << " seconds" << std::endl;
+ std::cout << "Min. measured delay during Solution: " << minDelayMeasured_ << " seconds" << std::endl;
+#endif
+
+ if (firstRun_)
+ {
+ // start timer for measuring how much time elapsed since the first successful plan
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.start(externalTime);
+ }
+ else
+ {
+ firstSolveTimer_.start();
+ }
+
+ firstRun_ = false;
+ }
+ }
+
+
+ //! retrieve the time that elapsed since the last successful solve() call to an Optimal Control Problem
+ /*!
+ * the returned time can be used to synchronize the calls to optimal control problems
+ * @return time elapsed
+ */
+ SCALAR timeSincePreviousSuccessfulSolve(const SCALAR& externalTime)
+ {
+ if (firstRun_)
+ return 0.0;
+ else
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_lastSolveTimer_.stop(externalTime);
+ return ext_lastSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ lastSolveTimer_.stop();
+ return lastSolveTimer_.getElapsedTime();
+ }
+ }
+ }
+
+
+ //! retrieve the time that elapsed since the first successful solve() call to an Optimal Control Problem
+ /*!
+ * the returned time can be used externally, for example to update cost functions
+ * @return time elapsed
+ */
+ const SCALAR timeSinceFirstSuccessfulSolve(const SCALAR& externalTime)
+ {
+ if (firstRun_)
+ return 0.0;
+ else
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.stop(externalTime);
+ return ext_firstSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ firstSolveTimer_.stop();
+ return firstSolveTimer_.getElapsedTime();
+ }
+ }
+ }
+
+ //! obtain the delay which was measured during solving the optimal control problem
+ const SCALAR& getMeasuredDelay() const { return lastMeasuredDelay_; }
+ //! get the maximum measured delay (maximum over all cycles)
+ const SCALAR& getMaxMeasuredDelay() const { return maxDelayMeasured_; }
+ //! get the smallest measured delay (minimum over all cycles)
+ const SCALAR& getMinMeasuredDelay() const { return minDelayMeasured_; }
+ //! get the sum of all measured delays
+ const SCALAR& getSummedDelay() const { return summedDelay_; }
+private:
+ //! computes the time to forward integrate a measured state according to the current policy based on fixed delays and measured delays
+ /*!
+ * The delay to be applied consists of a fixed-time delay specified by the user (for example communication delays)
+ * and a variable delay, which is based on a delay-measurement, which captures the optimal control problem solving times.
+ * The delay to be applied is the sum of fixed and variable components.
+ * @return delay to be applied
+ */
+ SCALAR computeDelayToApply()
+ {
+ SCALAR fixedDelay = 1e-6 * mpc_settings_.additionalDelayUs_;
+ SCALAR variableDelay = 0.0;
+
+ if (mpc_settings_.measureDelay_) // use variable, measured delay
+ variableDelay = mpc_settings_.delayMeasurementMultiplier_ * lastMeasuredDelay_;
+ else // using fixed delay
+ fixedDelay += 1e-6 * mpc_settings_.fixedDelayUs_;
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "Accumulated delay to apply: " << fixedDelay + variableDelay << " seconds" << std::endl;
+#endif
+
+ return fixedDelay + variableDelay;
+ }
+
+
+ mpc_settings mpc_settings_;
+
+ bool initialized_;
+
+ //! flag indicating that the time horizon has been hit
+ bool finalPointReached_;
+
+ //! timer for internally measuring the time elapsed during planning, internal, relative time
+ core::tpl::Timer<SCALAR> timer_;
+ //! timer to internally measure how much time elapsed since the last finished solve
+ core::tpl::Timer<SCALAR> lastSolveTimer_;
+ //! timer for internally measuring how much time elapsed since the first successful plan
+ core::tpl::Timer<SCALAR> firstSolveTimer_;
+
+ //! timer for internally measuring the time elapsed during planning, internal, relative time
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_timer_;
+ //! timer to internally measure how much time elapsed since the last finished solve
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_lastSolveTimer_;
+ //! timer for internally measuring how much time elapsed since the first successful plan
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_firstSolveTimer_;
+
+ SCALAR lastMeasuredDelay_; //! last delay in planning, internal, relative time
+ SCALAR maxDelayMeasured_; //! the max. delay occurred due to planning
+ SCALAR minDelayMeasured_; //! the min. delay occurred due to planning
+ SCALAR summedDelay_; //! sum of all delays measured
+
+ //! time horizon strategy specified by the user, e.g. constant receding horizon
+ std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy_;
+
+ bool firstRun_; //! set to true if first run active
+};
+
+
+} // namespace tpl
+
+typedef tpl::MpcTimeKeeper<double> MpcTimeKeeper;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h
new file mode 100644
index 0000000..2c8ee36
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h
@@ -0,0 +1,44 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::PolicyHandler()
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::~PolicyHandler()
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& TimeHorizon,
+ POLICY& policy)
+{
+ policy = initialPolicy_;
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::truncateSolutionFront(const SCALAR& delay,
+ POLICY& policy,
+ SCALAR& effectivelyTruncated)
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::setPolicy(const POLICY& newPolicy)
+{
+ initialPolicy_ = newPolicy;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h
new file mode 100644
index 0000000..68d2cff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h
@@ -0,0 +1,61 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class PolicyHandler
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ PolicyHandler();
+
+ virtual ~PolicyHandler();
+
+ //! design a warm-starting policy for the optimal control problem solver
+ /*!
+ * Designs an initial guess for MPC (warm-start). An optimal strategy for warm-starting might be highly application/system dependent,
+ * thus the user can overload this method if desired. Straight-forward default implementations for common 'ct' solvers and policy
+ * types are provided in the folder "default".
+ * Note that the default policy handler simply performs "cold-starting" which means that the initially provided control policy is returned without modification.
+ *
+ * @param delay
+ * time difference between nominal starting time of the current policy and when the warm-start policy should start
+ * @param TimeHorizon
+ * desired overall policy time horizon (note: not covering the whole time-horizon may result in an error)
+ * @param policy
+ * the current policy, to be overwritten with the warm start
+ */
+ virtual void designWarmStartingPolicy(const SCALAR& delay, const SCALAR& TimeHorizon, POLICY& policy);
+
+
+ //! a method required for additional post-truncation.
+ /*!
+ * post truncation may become relevant if the delay is underestimated or pre-integration is turned off.
+ * @param delay
+ * the time to truncate away from the solution
+ * @param policy
+ * the policy to be truncated
+ * @param effectivelyTruncated
+ * the time which was truncated away
+ */
+ virtual void truncateSolutionFront(const SCALAR& delay, POLICY& policy, SCALAR& effectivelyTruncated);
+
+
+ //! set new policy to policy handler
+ void setPolicy(const POLICY& newPolicy);
+
+
+protected:
+ POLICY initialPolicy_; //! the initial policy
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
new file mode 100644
index 0000000..4eaa32b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
@@ -0,0 +1,121 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::StateFeedbackPolicyHandler(const SCALAR& dt) : dt_(dt)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::~StateFeedbackPolicyHandler()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& newTimeHorizon,
+ StateFeedbackController_t& policy)
+{
+ // get the current reference trajectories from the StateFeedbackController
+ core::FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>& FeedbackTraj = policy.getFeedbackTrajectory();
+ core::ControlTrajectory<CONTROL_DIM, SCALAR>& FeedForwardTraj = policy.getFeedforwardTrajectory();
+ core::StateTrajectory<STATE_DIM, SCALAR>& StateRefTraj = policy.getReferenceStateTrajectory();
+
+ // current number of discrete elements
+ int currentSize = FeedForwardTraj.size();
+
+ // compute new controller length as a function of the time horizon
+ int Kn_new = std::max(1, (int)std::lround(newTimeHorizon / dt_));
+
+ // compute number indices to be shifted. Note: it does not make sense to shift more entries than are available
+ int num_di = FeedForwardTraj.getIndexFromTime(delay);
+ num_di = std::min(num_di, currentSize - 1);
+
+
+#ifdef DEBUG_POLICYHANDLER
+ std::cout << "DEBUG_POLICYHANDLER: Controller shifting: " << std::endl
+ << "delay: " << delay << " newT: " << newTimeHorizon << std::endl
+ << " new Discrete Controller has: " << std::endl
+ << Kn_new << " control elements, shifted about " << num_di << " elements." << std::endl
+ << Kn_new + 1 << " state elements, shifted about " << num_di << " elements." << std::endl;
+#endif
+
+
+ // Step 1 - Truncate Front: remove first 'num_di' elements from controller and shift time accordingly
+ if (num_di > 0)
+ {
+ FeedForwardTraj.eraseFront(num_di, num_di * dt_);
+ FeedbackTraj.eraseFront(num_di, num_di * dt_);
+ StateRefTraj.eraseFront(num_di, num_di * dt_);
+ currentSize -= num_di;
+ }
+
+
+ // Step 2 - Resize overall controller
+ if (Kn_new > currentSize)
+ {
+ //extend at back with constant value taken from last element
+ bool timeIsRelative = true;
+ for (size_t i = 0; i < Kn_new - currentSize; i++)
+ {
+ FeedbackTraj.push_back(FeedbackTraj.back(), dt_, timeIsRelative);
+ FeedForwardTraj.push_back(FeedForwardTraj.back(), dt_, timeIsRelative);
+ StateRefTraj.push_back(StateRefTraj.back(), dt_, timeIsRelative);
+ }
+ }
+ else if (Kn_new < currentSize)
+ {
+ // remove elements from back
+ for (size_t i = 0; i < currentSize - Kn_new; i++)
+ {
+ FeedbackTraj.pop_back();
+ FeedForwardTraj.pop_back();
+ StateRefTraj.pop_back();
+ }
+ }
+
+ // safety check, which should never be entered
+ if (FeedForwardTraj.size() == 0)
+ {
+ throw std::runtime_error("ERROR in StateFeedbackPolicyHandler.h: new policy should not have size 0.");
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::truncateSolutionFront(const SCALAR& delay,
+ StateFeedbackController_t& policy,
+ SCALAR& effectivelyTruncated)
+{
+ // current controller length
+ size_t currentSize = policy.getFeedforwardTrajectory().size();
+
+ size_t num_di = policy.getFeedforwardTrajectory().getIndexFromTime(delay);
+ num_di = std::min(num_di, currentSize - 1);
+
+ effectivelyTruncated = num_di * dt_;
+
+#ifdef DEBUG_POLICYHANDLER
+ std::cout << "DEBUG_WARMSTART: Current Controller Size: " << currentSize << " elements." << std::endl;
+ std::cout << "DEBUG_WARMSTART: Controller truncation: truncation about " << num_di << " elements." << std::endl;
+ std::cout << "DEBUG_WARMSTART: Controller new size: " << currentSize - num_di << " elements." << std::endl;
+#endif
+
+ // remove first num_di elements from controller
+ if (num_di > 0 && num_di < currentSize)
+ {
+ policy.getFeedbackTrajectory().eraseFront(num_di, effectivelyTruncated);
+ policy.getFeedforwardTrajectory().eraseFront(num_di, effectivelyTruncated);
+ policy.getReferenceStateTrajectory().eraseFront(num_di, effectivelyTruncated);
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h
new file mode 100644
index 0000000..3727254
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h
@@ -0,0 +1,48 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+//! the default policy handler for iLQR
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+class StateFeedbackPolicyHandler : public PolicyHandler<core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> StateFeedbackController_t;
+
+ StateFeedbackPolicyHandler(const SCALAR& dt);
+
+ virtual ~StateFeedbackPolicyHandler();
+
+ virtual void designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& newTimeHorizon,
+ StateFeedbackController_t& policy) override;
+
+ /*!
+ * required for additional post-truncation.
+ * @param delay the delay which is to be truncated away
+ * @param policy the resulting, truncated policy
+ * @param effectivelyTruncated the time which was effectively truncated away
+ * (can be different from the input in discrete-time case, for example)
+ */
+ virtual void truncateSolutionFront(const SCALAR& delay,
+ StateFeedbackController_t& policy,
+ SCALAR& effectivelyTruncated) override;
+
+private:
+ SCALAR dt_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h
new file mode 100644
index 0000000..3adb3ea
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h
@@ -0,0 +1,91 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+template <typename SCALAR>
+MpcTimeHorizon<SCALAR>::MpcTimeHorizon(const mpc_settings& settings, const SCALAR& initialTimeHorizon)
+ : mpc_settings_(settings), initialTimeHorizon_(initialTimeHorizon)
+{
+}
+
+template <typename SCALAR>
+MpcTimeHorizon<SCALAR>::~MpcTimeHorizon()
+{
+}
+
+template <typename SCALAR>
+bool MpcTimeHorizon<SCALAR>::computeNewTimeHorizon(const SCALAR& t_since_ended_first_solve,
+ const SCALAR& t_forward_prediction_stop,
+ SCALAR& new_T)
+{
+ /**
+ * compute desired end time and the time we have left from now. Will be ignored below, if not required in the scenario.
+ */
+ SCALAR timeLeft = initialTimeHorizon_ - (t_since_ended_first_solve + t_forward_prediction_stop);
+ timeLeft = std::max((SCALAR)0.0, timeLeft);
+
+
+ switch (mpc_settings_.mpc_mode)
+ {
+ case MPC_MODE::CONSTANT_RECEDING_HORIZON:
+ {
+ new_T = initialTimeHorizon_; // leave time horizon unchanged
+
+ return false; // in this mode, we never reach to the final time
+ }
+ case MPC_MODE::FIXED_FINAL_TIME:
+ {
+ new_T = timeLeft;
+
+ if (new_T == 0.0)
+ return true; // reached time horizon, return true
+
+ return false;
+ }
+ case MPC_MODE::FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON:
+ {
+ // std::max() ensures that the time is greater than the mininmum specified time horizon
+ new_T = std::max((SCALAR)mpc_settings_.minimumTimeHorizonMpc_, timeLeft);
+
+ if (new_T == mpc_settings_.minimumTimeHorizonMpc_)
+ return true;
+
+ return false;
+ }
+ case MPC_MODE::RECEDING_HORIZON_WITH_FIXED_FINAL_TIME:
+ {
+ new_T = std::min((SCALAR)mpc_settings_.minimumTimeHorizonMpc_, timeLeft);
+
+ if (new_T == 0.0)
+ return true;
+
+ return false;
+ }
+ default:
+ throw std::runtime_error("ERROR in MPC Constructor -- unknown Time Horizon Strategy.");
+ }
+}
+
+template <typename SCALAR>
+void MpcTimeHorizon<SCALAR>::updateSettings(const mpc_settings& mpcsettings)
+{
+ mpc_settings_ = mpcsettings;
+}
+
+template <typename SCALAR>
+void MpcTimeHorizon<SCALAR>::updateInitialTimeHorizon(const SCALAR& initTimeHorizon)
+{
+ initialTimeHorizon_ = initTimeHorizon;
+}
+
+} // namespace tpl
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h
new file mode 100644
index 0000000..c1750cf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h
@@ -0,0 +1,59 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "../MpcSettings.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/*!
+ * This class implements the four default strategies for the time horizon in ct's MPC.
+ * In case a different time horizon strategy is required, the user can derive from this
+ * class and override the virtual functions
+ */
+template <typename SCALAR = double>
+class MpcTimeHorizon
+{
+public:
+ MpcTimeHorizon(const mpc_settings& settings, const SCALAR& initialTimeHorizon);
+
+ virtual ~MpcTimeHorizon();
+
+ //! compute new MPC time horizon
+ /*!
+ * @param t_since_ended_first_solve
+ * Time since the first successful solve
+ * @param t_forward_prediction_stop
+ * Relative time where to stop forward integration w.r.t. previous controller
+ * @param new_T
+ * Resulting, new time horizon provided to the solver
+ * @return true if TimeHorizon reached and MPC should stop
+ */
+ virtual bool computeNewTimeHorizon(const SCALAR& t_since_ended_first_solve,
+ const SCALAR& t_forward_prediction_stop,
+ SCALAR& new_T);
+
+ void updateSettings(const mpc_settings& mpcsettings);
+
+ //! update the time horizon which is used during the first call to the solver
+ void updateInitialTimeHorizon(const SCALAR& initTimeHorizon);
+
+
+protected:
+ mpc_settings mpc_settings_;
+
+ SCALAR initialTimeHorizon_;
+};
+
+} // namespace tpl
+
+typedef tpl::MpcTimeHorizon<double> MpcTimeHorizon;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp
new file mode 100644
index 0000000..8ba1446
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp
@@ -0,0 +1,46 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCAlgorithm
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ NLOCAlgorithm(const std::shared_ptr<Backend_t>& backend) : backend_(backend) {}
+ virtual ~NLOCAlgorithm() {}
+ virtual void configure(const Settings_t& settings) = 0;
+
+ virtual void prepareIteration() = 0;
+
+ virtual bool finishIteration() = 0;
+
+ virtual bool runIteration() = 0;
+
+ virtual void setInitialGuess(const Policy_t& initialGuess) = 0;
+
+ virtual void prepareMPCIteration() = 0;
+
+ virtual bool finishMPCIteration() = 0;
+
+protected:
+ std::shared_ptr<Backend_t> backend_;
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
new file mode 100644
index 0000000..d68bcc9
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
@@ -0,0 +1,1834 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#define SYMPLECTIC_ENABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V > 0 && P > 0 && (V+P==ST)), void>::type
+#define SYMPLECTIC_DISABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V <= 0 || P <= 0 || (V+P!=ST)), void>::type
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendBase(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const Settings_t& settings)
+ :
+
+ substepRecorders_(),
+ integrators_(settings.nThreads + 1),
+ sensitivity_(settings.nThreads + 1),
+ integratorsEulerSymplectic_(settings.nThreads + 1),
+ integratorsRkSymplectic_(settings.nThreads + 1),
+
+ controller_(settings.nThreads + 1),
+ initialized_(false),
+ configured_(false),
+ iteration_(0),
+ settings_(settings),
+ K_(0),
+ d_norm_(0.0),
+ e_box_norm_(0.0),
+ e_gen_norm_(0.0),
+ lx_norm_(0.0),
+ lu_norm_(0.0),
+ lqocProblem_(new LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>()),
+
+ substepsX_(new StateSubsteps),
+ substepsU_(new ControlSubsteps),
+
+ intermediateCostBest_(std::numeric_limits<SCALAR>::infinity()),
+ finalCostBest_(std::numeric_limits<SCALAR>::infinity()),
+ lowestCost_(std::numeric_limits<SCALAR>::infinity()),
+ intermediateCostPrevious_(std::numeric_limits<SCALAR>::infinity()),
+ finalCostPrevious_(std::numeric_limits<SCALAR>::infinity()),
+ linearSystems_(settings.nThreads + 1),
+ boxConstraints_(settings.nThreads + 1, nullptr), // initialize constraints with null
+ generalConstraints_(settings.nThreads + 1, nullptr), // initialize constraints with null
+ firstRollout_(true),
+ alphaBest_(-1),
+ lqpCounter_(0)
+{
+ Eigen::initParallel();
+
+ for (int i = 0; i < settings.nThreads + 1; i++)
+ {
+ controller_[i] = ConstantControllerPtr(new core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>());
+ }
+
+ configure(settings);
+
+ changeTimeHorizon(optConProblem.getTimeHorizon());
+ changeInitialState(optConProblem.getInitialState());
+ changeCostFunction(optConProblem.getCostFunction());
+ changeNonlinearSystem(optConProblem.getNonlinearSystem());
+ changeLinearSystem(optConProblem.getLinearSystem());
+
+ // if the optimal problem has constraints, change
+ if (optConProblem.getBoxConstraints())
+ changeBoxConstraints(optConProblem.getBoxConstraints());
+ if (optConProblem.getGeneralConstraints())
+ changeGeneralConstraints(optConProblem.getGeneralConstraints());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendBase(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : NLOCBackendBase(optConProblem, Settings_t::fromConfigFile(settingsFile, verbose, ns))
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendBase()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ if (initialGuess.x_ref().size() < (size_t)K_ + 1)
+ {
+ std::cout << "Initial state guess length too short. Received length " << initialGuess.x_ref().size()
+ << ", expected " << K_ + 1 << std::endl;
+ throw(std::runtime_error("initial state guess to short"));
+ }
+
+ if (initialGuess.uff().size() < (size_t)K_)
+ {
+ std::cout << "Initial control guess length too short. Received length " << initialGuess.uff().size()
+ << ", expected " << K_ << std::endl;
+ throw std::runtime_error("initial control guess to short");
+ }
+
+ if (initialGuess.K().size() < (size_t)K_)
+ {
+ std::cout << "Initial feedback length too short. Received length " << initialGuess.K().size() << ", expected "
+ << K_ << std::endl;
+ throw std::runtime_error("initial control guess to short");
+ }
+
+ u_ff_ = initialGuess.uff();
+ L_ = initialGuess.K();
+ x_ = initialGuess.x_ref();
+ x_prev_ = x_;
+
+ if (x_.size() > (size_t)K_ + 1)
+ {
+ std::cout << "Warning, initial state guess too long. Received length " << x_.size() << ", expected " << K_ + 1
+ << ", will truncate" << std::endl;
+ x_.resize(K_ + 1);
+ }
+
+ if (u_ff_.size() > (size_t)K_)
+ {
+ std::cout << "Warning, initial control guess too long. Received length " << u_ff_.size() << ", expected " << K_
+ << ", will truncate" << std::endl;
+ u_ff_.resize(K_);
+ }
+
+ if (L_.size() > (size_t)K_)
+ {
+ std::cout << "Warning, initial feedback guess too long. Received length " << L_.size() << ", expected " << K_
+ << ", will truncate" << std::endl;
+ L_.resize(K_);
+ }
+
+ initialized_ = true;
+
+ t_ = TimeArray(settings_.dt, x_.size(), 0.0);
+
+ reset();
+
+ // compute costs of the initial guess trajectory
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+ intermediateCostPrevious_ = intermediateCostBest_;
+ finalCostPrevious_ = finalCostBest_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeTimeHorizon(const SCALAR& tf)
+{
+ if (tf < 0)
+ throw std::runtime_error("negative time horizon specified");
+
+ int newK = settings_.computeK(tf);
+
+ if (newK == K_)
+ return;
+
+ K_ = newK;
+
+ t_ = TimeArray(settings_.dt, K_ + 1, 0.0);
+
+ lx_.resize(K_ + 1);
+ x_.resize(K_ + 1);
+ x_prev_.resize(K_ + 1);
+ xShot_.resize(K_ + 1);
+
+ lu_.resize(K_);
+ u_ff_.resize(K_);
+ u_ff_prev_.resize(K_);
+ L_.resize(K_);
+
+ substepsX_->resize(K_ + 1);
+ substepsU_->resize(K_ + 1);
+
+ lqocProblem_->changeNumStages(K_);
+ lqocProblem_->setZero();
+
+ lqocSolver_->setProblem(lqocProblem_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeInitialState(
+ const core::StateVector<STATE_DIM, SCALAR>& x0)
+{
+ if (x_.size() == 0)
+ x_.resize(1);
+
+ x_[0] = x0;
+ reset(); // since initial state changed, we have to start fresh, i.e. with a rollout
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeCostFunction(
+ const typename OptConProblem_t::CostFunctionPtr_t& cf)
+{
+ if (cf == nullptr)
+ throw std::runtime_error("cost function is nullptr");
+
+ costFunctions_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ costFunctions_[i] = typename OptConProblem_t::CostFunctionPtr_t(cf->clone());
+ }
+
+ // recompute cost if line search is active
+ // TODO: this should be multi-threaded to save time
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeNonlinearSystem(
+ const typename OptConProblem_t::DynamicsPtr_t& dyn)
+{
+ if (dyn == nullptr)
+ throw std::runtime_error("system dynamics are nullptr");
+
+ systems_.resize(settings_.nThreads + 1);
+ substepRecorders_.resize(settings_.nThreads + 1);
+ integrators_.resize(settings_.nThreads + 1);
+ sensitivity_.resize(settings_.nThreads + 1);
+ integratorsEulerSymplectic_.resize(settings_.nThreads + 1);
+ integratorsRkSymplectic_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ systems_[i] = typename OptConProblem_t::DynamicsPtr_t(dyn->clone());
+ systems_[i]->setController(controller_[i]);
+
+ substepRecorders_[i] = std::shared_ptr<ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>(systems_[i]));
+
+ if (controller_[i] == nullptr)
+ throw std::runtime_error("Controller not defined");
+
+ // if symplectic integrator then don't create normal ones
+ if (settings_.integrator != ct::core::IntegrationType::EULER_SYM &&
+ settings_.integrator != ct::core::IntegrationType::RK_SYM)
+ {
+ integrators_[i] = std::shared_ptr<ct::core::Integrator<STATE_DIM, SCALAR>>(
+ new ct::core::Integrator<STATE_DIM, SCALAR>(systems_[i], settings_.integrator, substepRecorders_[i]));
+ }
+ initializeSymplecticIntegrators<V_DIM, P_DIM, STATE_DIM>(i);
+
+
+ if (settings_.useSensitivityIntegrator)
+ {
+ if (settings_.integrator != ct::core::IntegrationType::EULER &&
+ settings_.integrator != ct::core::IntegrationType::EULERCT &&
+ settings_.integrator != ct::core::IntegrationType::RK4 &&
+ settings_.integrator != ct::core::IntegrationType::RK4CT &&
+ settings_.integrator != ct::core::IntegrationType::EULER_SYM)
+ throw std::runtime_error("sensitivity integrator only available for Euler and RK4 integrators");
+
+ sensitivity_[i] =
+ SensitivityPtr(new ct::core::SensitivityIntegrator<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(
+ settings_.getSimulationTimestep(), linearSystems_[i], controller_[i], settings_.integrator,
+ settings_.timeVaryingDiscretization));
+ }
+ else
+ {
+ sensitivity_[i] =
+ SensitivityPtr(new ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(
+ settings_.dt, linearSystems_[i], settings_.discretization));
+ }
+ }
+ reset(); // since system changed, we have to start fresh, i.e. with a rollout
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeBoxConstraints(
+ const typename OptConProblem_t::ConstraintPtr_t& con)
+{
+ if (con == nullptr)
+ throw std::runtime_error("NLOCBackendBase: box constraint is nullptr");
+
+ boxConstraints_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ boxConstraints_[i] = typename OptConProblem_t::ConstraintPtr_t(con->clone());
+ }
+
+ // TODO can we do this multi-threaded?
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeGeneralConstraints(
+ const typename OptConProblem_t::ConstraintPtr_t& con)
+{
+ if (con == nullptr)
+ throw std::runtime_error("NLOCBackendBase: general constraint is nullptr");
+
+ generalConstraints_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ generalConstraints_[i] = typename OptConProblem_t::ConstraintPtr_t(con->clone());
+ }
+
+ // TODO can we do this multi-threaded?
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_ENABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeSymplecticIntegrators(
+ size_t i)
+{
+ if (systems_[i]->isSymplectic())
+ {
+ //! it only makes sense to compile the following code, if V_DIM > 0 and P_DIM > 0
+ integratorsEulerSymplectic_[i] =
+ std::shared_ptr<ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>(
+ std::static_pointer_cast<ct::core::SymplecticSystem<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ systems_[i]),
+ substepRecorders_[i]));
+ integratorsRkSymplectic_[i] =
+ std::shared_ptr<ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>(
+ std::static_pointer_cast<ct::core::SymplecticSystem<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ systems_[i])));
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_DISABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeSymplecticIntegrators(
+ size_t i)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeLinearSystem(
+ const typename OptConProblem_t::LinearPtr_t& lin)
+{
+ linearSystems_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ linearSystems_[i] = typename OptConProblem_t::LinearPtr_t(lin->clone());
+
+ sensitivity_[i]->setLinearSystem(linearSystems_[i]);
+ }
+ // technically a linear system change does not require a new rollout. Hence, we do not reset.
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::checkProblem()
+{
+ if (K_ == 0)
+ throw std::runtime_error("Time horizon too small resulting in 0 steps");
+
+ if (u_ff_.size() < (size_t)K_)
+ {
+ std::cout << "Provided initial feed forward controller too short, should be at least " << K_ << " but is "
+ << u_ff_.size() << " long." << std::endl;
+ throw(std::runtime_error("Provided initial feed forward controller too short"));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ if (!settings.parametersOk())
+ {
+ throw(std::runtime_error("Settings are incorrect. Aborting."));
+ }
+
+ if (settings.nThreads != settings_.nThreads)
+ {
+ throw(std::runtime_error("Number of threads cannot be changed after instance has been created."));
+ }
+
+ // update system discretizer with new settings
+ for (int i = 0; i < settings.nThreads + 1; i++)
+ {
+ if (!sensitivity_[i])
+ break;
+
+ sensitivity_[i]->setApproximation(settings.discretization);
+
+ if (settings.useSensitivityIntegrator)
+ sensitivity_[i]->setTimeDiscretization(settings.getSimulationTimestep());
+ else
+ sensitivity_[i]->setTimeDiscretization(settings.dt);
+ }
+
+ if (settings.integrator != settings_.integrator)
+ {
+ throw std::runtime_error("Cannot change integration type.");
+ }
+
+ // select the linear quadratic solver based on settings file
+ if (settings.lqocp_solver == NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ lqocSolver_ = std::shared_ptr<GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>());
+ }
+ else if (settings.lqocp_solver == NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+#ifdef HPIPM
+ lqocSolver_ =
+ std::shared_ptr<HPIPMInterface<STATE_DIM, CONTROL_DIM>>(new HPIPMInterface<STATE_DIM, CONTROL_DIM>());
+#else
+ throw std::runtime_error("HPIPM selected but not built.");
+#endif
+ }
+ else
+ throw std::runtime_error("Solver for Linear Quadratic Optimal Control Problem wrongly specified.");
+
+ // set number of Eigen Threads (requires -fopenmp)
+ if (settings_.nThreadsEigen > 1)
+ {
+ Eigen::setNbThreads(settings.nThreadsEigen);
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP] Eigen using " << Eigen::nbThreads() << " threads." << std::endl;
+#endif
+ }
+
+ lqocSolver_->configure(settings);
+
+ settings_ = settings;
+
+ reset();
+
+ configured_ = true;
+}
+
+
+/*
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::simpleRollout(
+ const size_t threadId,
+ const ControlVectorArray& uff,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_recorded
+ )const
+{
+ const double& dt = settings_.dt;
+ const double dt_sim = settings_.getSimulationTimestep();
+ const size_t subSteps = settings_.K_sim;
+ const int K_local = K_;
+
+ u_recorded.clear();
+
+ x_local.resize(K_+1);
+ x_local.front() = x_ref_lqr.front();
+
+ if(uff.size() < (size_t)K_) throw std::runtime_error("simpleRollout: u_local is too short.");
+
+
+ for (int i = 1; i<K_local+1; i++)
+ {
+ x_local[i] = x_local[i-1];
+
+ u_recorded.push_back(uff[i-1] + L_[i-1] * (x_local[i-1] - x_ref_lqr[i-1]));
+
+ controller_[threadId]->setControl(u_recorded.back());
+
+
+ if(settings_.integrator == ct::core::IntegrationType::EULER_SYM || settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integrateSymplectic<V_DIM, P_DIM, STATE_DIM>(threadId, x_local[i], 0, subSteps, dt_sim);
+ } else
+ {
+ integrators_[threadId]->integrate_n_steps(x_local[i], 0, subSteps, dt_sim);
+ }
+
+
+ // check if nan
+ for (size_t k=0; k<STATE_DIM; k++)
+ {
+ if (isnan(x_local[i](k)))
+ {
+ x_local.resize(K_local+1, ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_recorded.resize(K_local, ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ return false;
+ }
+ }
+ for (size_t k=0; k<CONTROL_DIM; k++)
+ {
+ if (isnan(u_recorded.back()(k)))
+ {
+ x_local.resize(K_local+1, ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_recorded.resize(K_local, ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ std::cout << "control unstable" << std::endl;
+ return false;
+ }
+ }
+ }
+ return true;
+}
+*/
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutSingleShot(const size_t threadId,
+ const size_t k, //! the starting index of the shot
+ ControlVectorArray& u_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot, //! the value at the end of each integration interval
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ const double& dt = settings_.dt;
+ const double dt_sim = settings_.getSimulationTimestep();
+ const size_t subSteps = settings_.K_sim;
+ const int K_local = K_;
+
+ if (u_local.size() < (size_t)K_)
+ throw std::runtime_error("rolloutSingleShot: u_local is too short.");
+ if (x_local.size() < (size_t)K_ + 1)
+ throw std::runtime_error("rolloutSingleShot: x_local is too short.");
+ if (xShot.size() < (size_t)K_ + 1)
+ throw std::runtime_error("rolloutSingleShot: xShot is too short.");
+
+ xShot[k] = x_local[k]; // initialize
+
+ //! determine index where to stop at the latest
+ int K_stop = k + settings_.K_shot;
+ if (K_stop > K_local)
+ K_stop = K_local;
+
+ if (settings_.nlocp_algorithm == NLOptConSettings::NLOCP_ALGORITHM::ILQR)
+ K_stop = K_local; //! @todo this is not elegant - need to improve.
+
+ // for each control step
+ for (int i = (int)k; i < K_stop; i++)
+ {
+ if (terminationFlag && *terminationFlag)
+ return false;
+
+ substepRecorders_[threadId]->reset();
+
+ if (i > (int)k)
+ {
+ xShot[i] = xShot[i - 1]; //! initialize integration variable
+ }
+
+ // Todo: the order here is not optimal. In some cases, we will overwrite x_ref_lqr immediately in the next step!
+ if (settings_.closedLoopShooting) // overwrite control
+ u_local[i] += L_[i] * (xShot[i] - x_ref_lqr[i]);
+
+ //! @todo: here we override the state trajectory directly (as passed by reference). This is bad.
+ if (i > (int)k)
+ {
+ x_local[i] = xShot[i - 1]; //!"overwrite" x_local
+ }
+
+
+ controller_[threadId]->setControl(u_local[i]);
+
+
+ if (settings_.integrator == ct::core::IntegrationType::EULER_SYM ||
+ settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integrateSymplectic<V_DIM, P_DIM, STATE_DIM>(threadId, xShot[i], i * dt, subSteps, dt_sim);
+ }
+ else
+ {
+ integrators_[threadId]->integrate_n_steps(xShot[i], i * dt, subSteps, dt_sim);
+ }
+
+
+ if (i == K_local - 1)
+ x_local[K_local] = xShot[K_local - 1]; //! fill in terminal state if required
+
+
+ // check if nan
+ for (size_t j = 0; j < STATE_DIM; j++)
+ {
+ if (std::isnan(x_local[i](j)))
+ {
+ x_local.resize(K_local + 1,
+ ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_local.resize(K_local,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ return false;
+ }
+ }
+ for (size_t j = 0; j < CONTROL_DIM; j++)
+ {
+ if (std::isnan(u_local[i](j)))
+ {
+ x_local.resize(K_local + 1,
+ ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_local.resize(K_local,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ std::cout << "control unstable" << std::endl;
+ return false;
+ }
+ }
+
+ // get substeps for sensitivities
+ substepsX[i] = substepRecorders_[threadId]->getSubstates();
+ substepsU[i] = substepRecorders_[threadId]->getSubcontrols();
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_ENABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const
+{
+ if (!systems_[threadId]->isSymplectic())
+ throw std::runtime_error("Trying to integrate using symplectic integrator, but system is not symplectic.");
+
+ if (settings_.integrator == ct::core::IntegrationType::EULER_SYM)
+ {
+ integratorsEulerSymplectic_[threadId]->integrate_n_steps(x0, t, steps, dt_sim);
+ }
+ else if (settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integratorsRkSymplectic_[threadId]->integrate_n_steps(x0, t, steps, dt_sim);
+ }
+ else
+ {
+ throw std::runtime_error("invalid symplectic integrator specified");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_DISABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const
+{
+ throw std::runtime_error("Symplectic integrator selected but invalid dimensions for it. Check V_DIM>1, P_DIM>1");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShotsSingleThreaded(size_t threadId,
+ size_t firstIndex,
+ size_t lastIndex,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateVectorArray& d,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU) const
+{
+ //! make sure all intermediate entries in the defect trajectory are zero
+ d.setConstant(state_vector_t::Zero());
+
+ for (size_t k = firstIndex; k <= lastIndex; k = k + settings_.K_shot)
+ {
+ // first rollout the shot
+ bool dynamicsGood = rolloutSingleShot(threadId, k, u_ff_local, x_local, x_ref_lqr, xShot, substepsX, substepsU);
+
+ if (!dynamicsGood)
+ return false;
+
+ // then compute the corresponding defect
+ computeSingleDefect(k, x_local, xShot, d);
+ }
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeSingleDefect(size_t k,
+ const StateVectorArray& x_local,
+ const StateVectorArray& xShot,
+ StateVectorArray& d) const
+{
+ //! compute the index where the next shot starts (respect total number of stages)
+ int k_next = std::min(K_, (int)k + settings_.K_shot);
+
+ if (k_next < K_)
+ {
+ d[k_next - 1] = xShot[k_next - 1] - x_local[k_next];
+ }
+ //! else ... all other entries of d remain zero.
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeCostsOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost) const
+{
+ intermediateCost = 0;
+
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ // feed current state and control to cost function
+ costFunctions_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+
+ // derivative of cost with respect to state
+ intermediateCost += costFunctions_[threadId]->evaluateIntermediate();
+ }
+ intermediateCost *= settings_.dt;
+
+ costFunctions_[threadId]->setCurrentStateAndControl(x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ finalCost = costFunctions_[threadId]->evaluateTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeBoxConstraintErrorOfTrajectory(
+ size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const
+{
+ e_tot = 0;
+
+ // intermediate constraints
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ if (boxConstraints_[threadId] != nullptr)
+ {
+ if (boxConstraints_[threadId]->getIntermediateConstraintsCount() > 0)
+ {
+ boxConstraints_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+ Eigen::Matrix<SCALAR, -1, 1> box_err = boxConstraints_[threadId]->getTotalBoundsViolationIntermediate();
+ e_tot += box_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+ }
+
+ // normalize integrated violation by time
+ e_tot *= settings_.dt;
+
+ // terminal constraint violation
+ if (boxConstraints_[threadId] != nullptr)
+ {
+ if (boxConstraints_[threadId]->getTerminalConstraintsCount() > 0)
+ {
+ boxConstraints_[threadId]->setCurrentStateAndControl(
+ x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ Eigen::Matrix<SCALAR, -1, 1> box_err = boxConstraints_[threadId]->getTotalBoundsViolationTerminal();
+ e_tot += box_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeGeneralConstraintErrorOfTrajectory(
+ size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const
+{
+ e_tot = 0;
+
+ // intermediate constraints
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ if (generalConstraints_[threadId]->getIntermediateConstraintsCount() > 0)
+ {
+ generalConstraints_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+ Eigen::Matrix<SCALAR, -1, 1> gen_err =
+ generalConstraints_[threadId]->getTotalBoundsViolationIntermediate();
+ e_tot += gen_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+ }
+
+ // normalize integrated violation by time
+ e_tot *= settings_.dt;
+
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ if (generalConstraints_[threadId]->getTerminalConstraintsCount() > 0)
+ {
+ generalConstraints_[threadId]->setCurrentStateAndControl(
+ x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ Eigen::Matrix<SCALAR, -1, 1> gen_err = generalConstraints_[threadId]->getTotalBoundsViolationTerminal();
+ e_tot += gen_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLinearizedDynamics(size_t threadId, size_t k)
+{
+ LQOCProblem_t& p = *lqocProblem_;
+
+ /*!
+ * @todo
+ * Note the little 'hack' that is applied here. We need a control input to linearize around for the last stage N.
+ * Should it be zero? We currently set it to be the second-to-last control input.
+ */
+ const core::ControlVector<CONTROL_DIM, SCALAR> u_last = u_ff_[std::min((int)k + 1, K_ - 1)];
+
+ assert(lqocProblem_ != nullptr);
+
+ assert(&lqocProblem_->A_[k] != nullptr);
+ assert(&lqocProblem_->B_[k] != nullptr);
+
+ assert(lqocProblem_->A_.size() > k);
+ assert(lqocProblem_->B_.size() > k);
+
+ sensitivity_[threadId]->setSubstepTrajectoryReference(substepsX_.get(), substepsU_.get());
+ sensitivity_[threadId]->getAandB(x_[k], u_ff_[k], xShot_[k], (int)k, settings_.K_sim, p.A_[k], p.B_[k]);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setBoxConstraintsForLQOCProblem()
+{
+ // set box constraints if there are any
+ if (boxConstraints_[settings_.nThreads] != nullptr)
+ {
+ // temp vars
+ Eigen::VectorXi foo, u_sparsity_intermediate, x_sparsity_intermediate, x_sparsity_terminal;
+
+ // intermediate box constraints
+ const int nb_ux_intermediate = boxConstraints_[settings_.nThreads]->getJacobianStateNonZeroCountIntermediate() +
+ boxConstraints_[settings_.nThreads]->getJacobianInputNonZeroCountIntermediate();
+
+ if (nb_ux_intermediate > 0)
+ {
+ boxConstraints_[settings_.nThreads]->sparsityPatternInputIntermediate(foo, u_sparsity_intermediate);
+ boxConstraints_[settings_.nThreads]->sparsityPatternStateIntermediate(foo, x_sparsity_intermediate);
+
+ Eigen::VectorXi ux_sparsity_intermediate(nb_ux_intermediate);
+ x_sparsity_intermediate.array() += CONTROL_DIM; // shift indices to match combined decision vector [u, x]
+ ux_sparsity_intermediate << u_sparsity_intermediate, x_sparsity_intermediate;
+
+ lqocProblem_->setIntermediateBoxConstraints(nb_ux_intermediate,
+ boxConstraints_[settings_.nThreads]->getLowerBoundsIntermediate(),
+ boxConstraints_[settings_.nThreads]->getUpperBoundsIntermediate(), ux_sparsity_intermediate);
+ }
+
+ // terminal box constraints
+ const int nb_x_terminal = boxConstraints_[settings_.nThreads]->getJacobianStateNonZeroCountTerminal();
+
+ if (nb_x_terminal > 0)
+ {
+ boxConstraints_[settings_.nThreads]->sparsityPatternStateTerminal(foo, x_sparsity_terminal);
+
+ lqocProblem_->setTerminalBoxConstraints(nb_x_terminal,
+ boxConstraints_[settings_.nThreads]->getLowerBoundsTerminal(),
+ boxConstraints_[settings_.nThreads]->getUpperBoundsTerminal(), x_sparsity_terminal);
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLinearizedConstraints(size_t threadId,
+ size_t k)
+{
+ // set general if there are any
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ LQOCProblem_t& p = *lqocProblem_;
+ const scalar_t& dt = settings_.dt;
+
+ // treat general constraints
+ generalConstraints_[threadId]->setCurrentStateAndControl(x_[k], u_ff_[k], dt * k);
+
+ p.ng_[k] = generalConstraints_[threadId]->getIntermediateConstraintsCount();
+ if (p.ng_[k] > 0)
+ {
+ p.hasGenConstraints_ = true;
+ p.C_[k] = generalConstraints_[threadId]->jacobianStateIntermediate();
+ p.D_[k] = generalConstraints_[threadId]->jacobianInputIntermediate();
+
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> g_eval = generalConstraints_[threadId]->evaluateIntermediate();
+
+ // rewrite constraint in absolute coordinates as required by LQOC problem
+ p.d_lb_[k] = generalConstraints_[threadId]->getLowerBoundsIntermediate() - g_eval + p.C_[k] * x_[k] +
+ p.D_[k] * u_ff_[k];
+ p.d_ub_[k] = generalConstraints_[threadId]->getUpperBoundsIntermediate() - g_eval + p.C_[k] * x_[k] +
+ p.D_[k] * u_ff_[k];
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeQuadraticCosts(size_t threadId, size_t k)
+{
+ LQOCProblem_t& p = *lqocProblem_;
+ const scalar_t& dt = settings_.dt;
+
+ // feed current state and control to cost function
+ costFunctions_[threadId]->setCurrentStateAndControl(x_[k], u_ff_[k], dt * k);
+
+ // derivative of cost with respect to state
+ p.q_[k] = costFunctions_[threadId]->evaluateIntermediate() * dt;
+
+ p.qv_[k] = costFunctions_[threadId]->stateDerivativeIntermediate() * dt;
+
+ p.Q_[k] = costFunctions_[threadId]->stateSecondDerivativeIntermediate() * dt;
+
+ // derivative of cost with respect to control and state
+ p.P_[k] = costFunctions_[threadId]->stateControlDerivativeIntermediate() * dt;
+
+ // derivative of cost with respect to control
+ p.rv_[k] = costFunctions_[threadId]->controlDerivativeIntermediate() * dt;
+
+ p.R_[k] = costFunctions_[threadId]->controlSecondDerivativeIntermediate() * dt;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeCostToGo()
+{
+ LQOCProblem_t& p = *lqocProblem_;
+
+ // feed current state and control to cost function
+ costFunctions_[settings_.nThreads]->setCurrentStateAndControl(x_[K_], control_vector_t::Zero(), settings_.dt * K_);
+
+ // derivative of terminal cost with respect to state
+ p.q_[K_] = costFunctions_[settings_.nThreads]->evaluateTerminal();
+ p.qv_[K_] = costFunctions_[settings_.nThreads]->stateDerivativeTerminal();
+ p.Q_[K_] = costFunctions_[settings_.nThreads]->stateSecondDerivativeTerminal();
+
+ // init terminal general constraints, if any
+ if (generalConstraints_[settings_.nThreads] != nullptr)
+ {
+ p.ng_[K_] = generalConstraints_[settings_.nThreads]->getTerminalConstraintsCount();
+ if (p.ng_[K_] > 0)
+ {
+ p.hasGenConstraints_ = true;
+ p.C_[K_] = generalConstraints_[settings_.nThreads]->jacobianStateTerminal();
+ p.D_[K_] = generalConstraints_[settings_.nThreads]->jacobianInputTerminal();
+
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> g_eval =
+ generalConstraints_[settings_.nThreads]->evaluateTerminal();
+
+ p.d_lb_[K_] =
+ generalConstraints_[settings_.nThreads]->getLowerBoundsTerminal() - g_eval + p.C_[K_] * x_[K_];
+ p.d_ub_[K_] =
+ generalConstraints_[settings_.nThreads]->getUpperBoundsTerminal() - g_eval + p.C_[K_] * x_[K_];
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::printSummary()
+{
+ SCALAR d_norm_l1 = computeDefectsNorm<1>(lqocProblem_->b_);
+ SCALAR d_norm_l2 = computeDefectsNorm<2>(lqocProblem_->b_);
+ SCALAR totalCost = intermediateCostBest_ + finalCostBest_;
+ SCALAR totalMerit = intermediateCostBest_ + finalCostBest_ + settings_.meritFunctionRho * d_norm_l1;
+
+ SCALAR smallestEigenvalue = 0.0;
+ if (settings_.recordSmallestEigenvalue && settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ smallestEigenvalue = lqocSolver_->getSmallestEigenvalue();
+ }
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+
+ summaryAllIterations_.iterations.push_back(iteration_);
+ summaryAllIterations_.defect_l1_norms.push_back(d_norm_l1);
+ summaryAllIterations_.defect_l2_norms.push_back(d_norm_l2);
+ summaryAllIterations_.e_box_norms.push_back(e_box_norm_);
+ summaryAllIterations_.e_gen_norms.push_back(e_gen_norm_);
+ summaryAllIterations_.lx_norms.push_back(lx_norm_);
+ summaryAllIterations_.lu_norms.push_back(lu_norm_);
+ summaryAllIterations_.intermediateCosts.push_back(intermediateCostBest_);
+ summaryAllIterations_.finalCosts.push_back(finalCostBest_);
+ summaryAllIterations_.totalCosts.push_back(totalCost);
+ summaryAllIterations_.merits.push_back(totalMerit);
+ summaryAllIterations_.stepSizes.push_back(alphaBest_);
+ summaryAllIterations_.smallestEigenvalues.push_back(smallestEigenvalue);
+
+ if (settings_.printSummary)
+ summaryAllIterations_.printSummaryLastIteration();
+
+ //! @todo the printing of the smallest eigenvalue is hacky
+ if (settings_.printSummary && settings_.recordSmallestEigenvalue &&
+ settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ std::cout << std::setprecision(15) << "smallest eigenvalue this iteration: " << smallestEigenvalue << std::endl;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logToMatlab(const size_t& iteration)
+{
+#ifdef MATLAB_FULL_LOG
+
+ std::cout << "Logging to Matlab" << std::endl;
+
+ LQOCProblem_t& p = *lqocProblem_;
+
+ matFile_.open(settings_.loggingPrefix + "Log" + std::to_string(iteration) + ".mat");
+
+ matFile_.put("iteration", iteration);
+ matFile_.put("K", K_);
+ matFile_.put("dt", settings_.dt);
+ matFile_.put("K_sim", settings_.K_sim);
+ matFile_.put("x", x_.toImplementation());
+ matFile_.put("u_ff", u_ff_.toImplementation());
+ matFile_.put("t", t_.toEigenTrajectory());
+ matFile_.put("d", lqocProblem_->b_.toImplementation());
+ matFile_.put("xShot", xShot_.toImplementation());
+
+ matFile_.put("A", p.A_.toImplementation());
+ matFile_.put("B", p.B_.toImplementation());
+ matFile_.put("qv", p.qv_.toImplementation());
+ matFile_.put("Q", p.Q_.toImplementation());
+ matFile_.put("P", p.P_.toImplementation());
+ matFile_.put("rv", p.rv_.toImplementation());
+ matFile_.put("R", p.R_.toImplementation());
+ matFile_.put("q", p.q_.toEigenTrajectory());
+
+ matFile_.put("lx_norm", lx_norm_);
+ matFile_.put("lu_norm", lu_norm_);
+ matFile_.put("cost", getCost());
+ matFile_.put("alphaStep", alphaBest_);
+
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+ matFile_.put("d_norm", d_norm_);
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+ matFile_.put("e_box_norm", e_box_norm_);
+ matFile_.put("e_gen_norm", e_gen_norm_);
+
+ matFile_.close();
+#endif //MATLAB_FULL_LOG
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logInitToMatlab()
+{
+#ifdef MATLAB
+
+ std::cout << "Logging init guess to Matlab" << std::endl;
+
+ matFile_.open(settings_.loggingPrefix + "LogInit.mat");
+
+ matFile_.put("K", K_);
+ matFile_.put("dt", settings_.dt);
+ matFile_.put("K_sim", settings_.K_sim);
+
+ matFile_.put("x", x_.toImplementation());
+ matFile_.put("u_ff", u_ff_.toImplementation());
+ matFile_.put("d", lqocProblem_->b_.toImplementation());
+ matFile_.put("cost", getCost());
+
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+ matFile_.put("d_norm", d_norm_);
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+ matFile_.put("e_box_norm", e_box_norm_);
+ matFile_.put("e_gen_norm", e_gen_norm_);
+
+ matFile_.close();
+#endif
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::ControlTrajectory<CONTROL_DIM, SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlTrajectory() const
+{
+ // \todo this method currently copies the time array (suboptimal)
+
+ core::tpl::TimeArray<SCALAR> t_control = t_;
+ t_control.pop_back();
+
+ return core::ControlTrajectory<CONTROL_DIM, SCALAR>(t_control, u_ff_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::StateTrajectory<STATE_DIM, SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateTrajectory() const
+{
+ //! \todo this method currently copies the time array (suboptimal)
+
+ core::tpl::TimeArray<SCALAR> t_control = t_;
+
+ return core::StateTrajectory<STATE_DIM, SCALAR>(t_control, x_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCost() const
+{
+ return intermediateCostBest_ + finalCostBest_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchSingleShooting()
+{
+ // lowest cost is cost of last rollout
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+ scalar_t lowestCostPrevious = lowestCost_;
+
+ //! backup controller that led to current trajectory
+ u_ff_prev_ = u_ff_; // todo note this might be redundant
+ x_prev_ = x_; // todo note this might be redundant
+
+ if (!settings_.lineSearchSettings.active)
+ {
+ StateVectorArray x_ref_lqr_local(K_ + 1);
+ ControlVectorArray uff_local(K_);
+
+ uff_local = u_ff_ + lu_; // add lu
+ x_ref_lqr_local = x_prev_ + lx_; // stabilize around current solution candidate
+
+ bool dynamicsGood = rolloutSingleShot(
+ settings_.nThreads, 0, uff_local, x_, x_ref_lqr_local, xShot_, *this->substepsX_, *this->substepsU_);
+
+ if (dynamicsGood)
+ {
+ intermediateCostBest_ = std::numeric_limits<scalar_t>::max();
+ finalCostBest_ = std::numeric_limits<scalar_t>::max();
+ computeCostsOfTrajectory(settings_.nThreads, x_, uff_local, intermediateCostBest_, finalCostBest_);
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+
+ if (settings_.printSummary)
+ {
+ //! compute l2 norms of state and control update
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, uff_local);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+ }
+ else
+ {
+#ifdef MATLAB // in case of no debug printing but still logging, need to compute them \
+ //! compute l2 norms of state and control update
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, uff_local);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+#endif
+ }
+
+ x_prev_ = x_;
+ u_ff_.swap(uff_local);
+ alphaBest_ = 1;
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: System became unstable!" << std::endl;
+ }
+ return false;
+ }
+ }
+ else
+ {
+ // merit of previous trajectory
+ lowestCost_ = intermediateCostBest_ + finalCostBest_ +
+ (e_box_norm_ + e_gen_norm_) * settings_.meritFunctionRhoConstraints;
+ lowestCostPrevious = lowestCost_;
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Starting line search." << std::endl;
+ std::cout << "[LineSearch]: Merit last rollout: " << lowestCost_ << std::endl;
+ }
+
+ alphaBest_ = performLineSearch();
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Best control found at alpha: " << alphaBest_ << " . Will use this control."
+ << std::endl;
+ }
+
+ if (alphaBest_ == 0.0)
+ {
+ if (settings_.debugPrint)
+ {
+ {
+ std::cout << "WARNING: No better control found. Converged." << std::endl;
+ }
+ return false;
+ }
+ }
+ }
+
+ if ((fabs((lowestCostPrevious - lowestCost_) / lowestCostPrevious)) > settings_.min_cost_improvement)
+ {
+ return true; //! found better cost
+ }
+
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: Cost last iteration: " << lowestCostPrevious << ", current cost: " << lowestCost_
+ << std::endl;
+ std::cout << "CONVERGED: Cost improvement ratio was: "
+ << (lowestCostPrevious - lowestCost_) / lowestCostPrevious
+ << "x, which is lower than convergence criteria: " << settings_.min_cost_improvement << std::endl;
+ }
+ return false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::executeLineSearchSingleShooting(
+ const size_t threadId,
+ const scalar_t alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ intermediateCost = std::numeric_limits<scalar_t>::infinity();
+ finalCost = std::numeric_limits<scalar_t>::infinity();
+ e_box_norm = 0.0;
+ e_gen_norm = 0.0;
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ StateVectorArray x_ref_lqr;
+ StateVectorArray xShot_local(K_ + 1); //! note this is currently only a dummy (\todo nicer solution?)
+
+ //! if stabilizing about new solution candidate, choose lu as feedforward increment and also increment x_prev_ by lx
+ u_local = lu_ * alpha + u_ff_prev_;
+ x_ref_lqr = lx_ * alpha + x_prev_;
+
+ bool dynamicsGood =
+ rolloutSingleShot(threadId, 0, u_local, x_local, x_ref_lqr, xShot_local, substepsX, substepsU, terminationFlag);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ if (dynamicsGood)
+ {
+ // compute cost specific to this alpha
+ computeCostsOfTrajectory(threadId, x_local, u_local, intermediateCost, finalCost);
+
+ // compute constraint violations specific to this alpha
+ if (boxConstraints_[threadId] != nullptr)
+ computeBoxConstraintErrorOfTrajectory(threadId, x_local, u_local, e_box_norm);
+ if (generalConstraints_[threadId] != nullptr)
+ computeGeneralConstraintErrorOfTrajectory(threadId, x_local, u_local, e_gen_norm);
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::string msg = std::string("dynamics not good, thread: ") + std::to_string(threadId);
+ std::cout << msg << std::endl;
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchMultipleShooting()
+{
+ // lowest cost
+ scalar_t lowestCostPrevious;
+
+ //! backup controller that led to current trajectory
+ u_ff_prev_ = u_ff_;
+ x_prev_ = x_;
+
+
+ if (!settings_.lineSearchSettings.active) //! do full step updates
+ {
+ //! lowest cost is cost of last rollout
+ lowestCostPrevious = intermediateCostBest_ + finalCostBest_;
+
+ //! update control and states
+ doFullStepUpdate();
+
+ resetDefects();
+
+ rolloutShots(0, K_ - 1);
+
+ updateCosts();
+
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+
+ if (settings_.printSummary)
+ {
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, u_ff_);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+ }
+ else
+ {
+#ifdef MATLAB
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, u_ff_);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+#endif
+ }
+ x_prev_ = x_;
+ alphaBest_ = 1;
+ }
+ else //! do line search over a merit function trading off costs and constraint violations
+ {
+ // merit of previous trajectory
+ lowestCost_ = intermediateCostBest_ + finalCostBest_ + d_norm_ * settings_.meritFunctionRho;
+ lowestCostPrevious = lowestCost_;
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Starting line search." << std::endl;
+ std::cout << "[LineSearch]: Cost of last rollout:\t" << intermediateCostBest_ + finalCostBest_ << std::endl;
+ std::cout << "[LineSearch]: Defect norm last rollout:\t" << d_norm_ << std::endl;
+ std::cout << "[LineSearch]: Merit of last rollout:\t" << lowestCost_ << std::endl;
+ }
+
+ alphaBest_ = performLineSearch();
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Best control found at alpha: " << alphaBest_ << ", with trade-off "
+ << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCostBest_ + finalCostBest_ << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << d_norm_ << std::endl;
+ }
+
+ if (alphaBest_ == 0.0)
+ {
+ if (settings_.debugPrint || settings_.printSummary)
+ {
+ std::cout << "WARNING: No better control found during line search. Converged." << std::endl;
+ }
+ return false;
+ }
+ }
+
+ if ((fabs((lowestCostPrevious - lowestCost_) / lowestCostPrevious)) > settings_.min_cost_improvement)
+ {
+ return true; //! found better cost
+ }
+
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: Cost last iteration: " << lowestCostPrevious << ", current cost: " << lowestCost_
+ << std::endl;
+ std::cout << "CONVERGED: Cost improvement ratio was: "
+ << fabs(lowestCostPrevious - lowestCost_) / lowestCostPrevious
+ << "x, which is lower than convergence criteria: " << settings_.min_cost_improvement << std::endl;
+ }
+ return false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::executeLineSearchMultipleShooting(
+ const size_t threadId,
+ const scalar_t alpha,
+ const ControlVectorArray& u_ff_update,
+ const StateVectorArray& x_update,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_shot_alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& defects_recorded,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_alpha,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& defectNorm,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ intermediateCost = std::numeric_limits<scalar_t>::max();
+ finalCost = std::numeric_limits<scalar_t>::max();
+ defectNorm = std::numeric_limits<scalar_t>::max();
+ e_box_norm = 0.0;
+ e_gen_norm = 0.0;
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! update feedforward
+ u_alpha = u_ff_update * alpha + u_ff_prev_;
+
+ //! update state decision variables
+ x_alpha = x_update * alpha + x_prev_;
+
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ bool dynamicsGood = rolloutShotsSingleThreaded(
+ threadId, 0, K_ - 1, u_alpha, x_alpha, x_alpha, x_shot_alpha, defects_recorded, substepsX, substepsU);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! compute defects norm
+ defectNorm = computeDefectsNorm<1>(defects_recorded);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! compute costs
+ if (dynamicsGood)
+ {
+ computeCostsOfTrajectory(threadId, x_alpha, u_alpha, intermediateCost, finalCost);
+
+ // compute constraint violations specific to this alpha
+ if (boxConstraints_[threadId] != nullptr)
+ computeBoxConstraintErrorOfTrajectory(threadId, x_alpha, u_alpha, e_box_norm);
+ if (generalConstraints_[threadId] != nullptr)
+ computeGeneralConstraintErrorOfTrajectory(threadId, x_alpha, u_alpha, e_gen_norm);
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::string msg = std::string("dynamics not good, thread: ") + std::to_string(threadId);
+ std::cout << msg << std::endl;
+ }
+ }
+
+ if (terminationFlag && *terminationFlag)
+ return;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareSolveLQProblem(size_t startIndex)
+{
+ lqpCounter_++;
+
+ // if solver is HPIPM, there's nothing to prepare
+ if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+ }
+ // if solver is GNRiccati - we iterate backward up to the first stage
+ else if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+ lqocSolver_->setProblem(lqocProblem_);
+
+ //iterate backward up to first stage
+ for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= startIndex; i--)
+ lqocSolver_->solveSingleStage(i);
+ }
+ else
+ throw std::runtime_error("unknown solver type in prepareSolveLQProblem()");
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishSolveLQProblem(size_t endIndex)
+{
+ lqpCounter_++;
+
+ // if solver is HPIPM, solve the full problem
+ if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+ solveFullLQProblem();
+ }
+ else if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ // if solver is GNRiccati, solve the first stage and get solution
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+ lqocSolver_->setProblem(lqocProblem_);
+
+ for (int i = endIndex; i >= 0; i--)
+ lqocSolver_->solveSingleStage(i);
+
+ lqocSolver_->computeStateAndControlUpdates();
+ }
+ else
+ throw std::runtime_error("unknown solver type in finishSolveLQProblem()");
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::solveFullLQProblem()
+{
+ lqpCounter_++;
+
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+
+ if (lqocProblem_->isBoxConstrained())
+ {
+ if (settings_.debugPrint)
+ std::cout << "LQ Problem has box constraints. Configuring box constraints now. " << std::endl;
+
+ lqocSolver_->configureBoxConstraints(lqocProblem_);
+ }
+ if (lqocProblem_->isGeneralConstrained())
+ {
+ if (settings_.debugPrint)
+ std::cout << "LQ Problem has general constraints. Configuring general constraints now. " << std::endl;
+
+ lqocSolver_->configureGeneralConstraints(lqocProblem_);
+ }
+
+ lqocSolver_->setProblem(lqocProblem_);
+
+ lqocSolver_->solve();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::updateCosts()
+{
+ intermediateCostPrevious_ = intermediateCostBest_;
+ finalCostPrevious_ = finalCostBest_;
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::retrieveLastLinearizedModel(StateMatrixArray& A,
+ StateControlMatrixArray& B)
+{
+ A = lqocProblem_->A_;
+ B = lqocProblem_->B_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Policy_t&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSolution()
+{
+ policy_.update(x_, u_ff_, L_, t_);
+ return policy_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::reset()
+{
+ firstRollout_ = true;
+ iteration_ = 0;
+ d_norm_ = std::numeric_limits<scalar_t>::infinity();
+ lx_norm_ = std::numeric_limits<scalar_t>::infinity();
+ lu_norm_ = std::numeric_limits<scalar_t>::infinity();
+ intermediateCostBest_ = std::numeric_limits<scalar_t>::infinity();
+ finalCostBest_ = std::numeric_limits<scalar_t>::infinity();
+ intermediateCostPrevious_ = std::numeric_limits<scalar_t>::infinity();
+ finalCostPrevious_ = std::numeric_limits<scalar_t>::infinity();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::doFullStepUpdate()
+{
+ u_ff_ += lu_;
+ x_ += lx_;
+
+ alphaBest_ = 1.0;
+
+ if (settings_.debugPrint || settings_.printSummary)
+ {
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(lx_);
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(lu_);
+ }
+ else
+ {
+#ifdef MATLAB
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(lx_);
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(lu_);
+#endif
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logSummaryToMatlab(const std::string& fileName)
+{
+ summaryAllIterations_.logToMatlab(fileName);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const SummaryAllIterations<SCALAR>& NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSummary() const
+{
+ return summaryAllIterations_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateUpdates()
+{
+ lx_ = lqocSolver_->getStateUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlUpdates()
+{
+ lu_ = lqocSolver_->getControlUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getFeedback()
+{
+ if (settings_.closedLoopShooting)
+ lqocSolver_->getFeedback(L_);
+ else
+ L_.setConstant(core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero()); // TODO should eventually go away
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::resetDefects()
+{
+ lqocProblem_->b_.setConstant(state_vector_t::Zero());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDefectsNorm()
+{
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+size_t& NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::iteration()
+{
+ return iteration_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::nominalRollout()
+{
+ bool success =
+ rolloutSingleShot(settings_.nThreads, 0, u_ff_, x_, x_prev_, xShot_, *this->substepsX_, *this->substepsU_);
+ x_prev_ = x_;
+ u_ff_prev_ = u_ff_;
+ firstRollout_ = false;
+ return success;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::TimeArray&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeArray()
+{
+ return t_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::isConfigured()
+{
+ return configured_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::isInitialized()
+{
+ return initialized_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTotalDefect() const
+{
+ return d_norm_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::testConsistency()
+{
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances()
+{
+ return systems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances() const
+{
+ return systems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances()
+{
+ return linearSystems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances() const
+{
+ return linearSystems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances()
+{
+ return costFunctions_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances() const
+{
+ return costFunctions_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances()
+{
+ return boxConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances() const
+{
+ return boxConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances()
+{
+ return generalConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances() const
+{
+ return generalConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeHorizon()
+{
+ return K_ * settings_.dt;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNumSteps()
+{
+ return K_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNumStepsPerShot()
+{
+ return settings_.K_shot;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Settings_t&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSettings() const
+{
+ return settings_;
+}
+
+} // namespace optcon
+} // namespace ct
+
+
+#undef SYMPLECTIC_ENABLED
+#undef SYMPLECTIC_DISABLED
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
new file mode 100644
index 0000000..2c7a641
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
@@ -0,0 +1,724 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <atomic>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include <ct/optcon/problem/LQOCProblem.hpp>
+
+#include <ct/optcon/solver/lqp/GNRiccatiSolver.hpp>
+#include <ct/optcon/solver/lqp/HPIPMInterface.hpp>
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+#include "NLOCResults.hpp"
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+#define SYMPLECTIC_ENABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V > 0 && P > 0 && (V+P==ST)), void>::type
+#define SYMPLECTIC_DISABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V <= 0 || P <= 0 || (V+P!=ST)), void>::type
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \ingroup GNMS
+ *
+ * \brief C++ implementation of GNMS.
+ *
+ * The implementation and naming is based on the reference below. In general, the code follows this convention:
+ * X <- Matrix (upper-case in paper)
+ * xv <- vector (lower-case bold in paper)
+ * x <- scalar (lower-case in paper)
+ *
+ */
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t state_dim = STATE_DIM;
+ static const size_t control_dim = CONTROL_DIM;
+
+ typedef NLOptConSettings Settings_t;
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+ typedef LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR> LQOCSolver_t;
+
+ typedef core::Sensitivity<STATE_DIM, CONTROL_DIM, SCALAR> Sensitivity_t;
+
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+ typedef std::shared_ptr<StateVectorArray> StateVectorArrayPtr;
+
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+ typedef std::shared_ptr<ControlVectorArray> ControlVectorArrayPtr;
+
+ typedef std::vector<StateVectorArrayPtr, Eigen::aligned_allocator<StateVectorArrayPtr>> StateSubsteps;
+ typedef std::shared_ptr<StateSubsteps> StateSubstepsPtr;
+
+ typedef std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>> ControlSubsteps;
+ typedef std::shared_ptr<ControlSubsteps> ControlSubstepsPtr;
+
+ using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM, SCALAR>;
+ using ControlMatrixArray = ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR>;
+ using StateMatrixArray = ct::core::StateMatrixArray<STATE_DIM, SCALAR>;
+ using StateControlMatrixArray = ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>;
+ using FeedbackArray = ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>;
+ using TimeArray = ct::core::tpl::TimeArray<SCALAR>;
+
+ using state_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>;
+ using control_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM>;
+ using control_state_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>;
+ using state_control_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using feedback_matrix_t = core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using scalar_t = SCALAR;
+ using scalar_array_t = std::vector<SCALAR, Eigen::aligned_allocator<SCALAR>>;
+
+
+ NLOCBackendBase(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const Settings_t& settings);
+
+ NLOCBackendBase(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ virtual ~NLOCBackendBase();
+
+ //! configure the solver
+ /**
+ * Configure the solver
+ * @param settings solver settings
+ */
+ virtual void configure(const Settings_t& settings);
+
+ //! get the current SLQsolver settings
+ const Settings_t& getSettings() const;
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ void setInitialGuess(const Policy_t& initialGuess);
+
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ void changeTimeHorizon(const SCALAR& tf);
+
+ SCALAR getTimeHorizon();
+
+ int getNumSteps();
+
+ int getNumStepsPerShot();
+
+ SYMPLECTIC_ENABLED initializeSymplecticIntegrators(size_t i);
+ SYMPLECTIC_DISABLED initializeSymplecticIntegrators(size_t i);
+
+ SYMPLECTIC_ENABLED integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const;
+ SYMPLECTIC_DISABLED integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const;
+
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ */
+ void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0);
+
+ /*!
+ * \brief Change the cost function
+ */
+ void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf);
+
+ /*!
+ * \brief Change the nonlinear system
+ */
+ void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn);
+
+ /*!
+ * \brief Change the linear system
+ */
+ void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin);
+
+ /*!
+ * \brief Change the box constraints
+ */
+ void changeBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);
+
+ /*!
+ * \brief Change the general constraints
+ */
+ void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);
+
+ /*!
+ * \brief Direct accessor to the system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeNonlinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances();
+
+ const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const;
+
+ /*!
+ * \brief Direct accessor to the linear system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeLinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances();
+
+ const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const;
+
+ /*!
+ * \brief Direct accessor to the cost function instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances();
+
+ const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const;
+
+ /**
+ * @brief Direct accessor to the box constraint instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The box constraint instances
+ */
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances();
+
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const;
+
+ /**
+ * @brief Direct accessor to the general constraints
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The general constraints instances.
+ */
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances();
+
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const;
+
+
+ /*!
+ * Tests consistency of the instance of the dynamics, linear systems and costs. This is not a test for thread safety.
+ * @return returns true if instances are consistent with each other
+ */
+ bool testConsistency();
+
+
+ //! Export all functions to matlab workspace
+ /*!
+ This function can be used for Debugging. It exports all variables to Matlab after each iteration. It also saves
+ the Matlab workspace to a .mat file.
+ */
+ void logToMatlab(const size_t& iteration);
+
+ //! log the initial guess to Matlab
+ void logInitToMatlab();
+
+ //! return the cost of the solution of the current iteration
+ SCALAR getCost() const;
+
+ //! return the sum of the L2-norm of the defects along the solution candidate
+ SCALAR getTotalDefect() const;
+
+ void reset();
+
+ const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const;
+
+ const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const;
+
+
+ const Policy_t& getSolution();
+
+ const TimeArray& getTimeArray();
+
+ bool isConfigured();
+
+ bool isInitialized();
+
+
+ //! Retrieve Last Linearized Model
+ /*!
+ Retrieve the linearized model computed during the last iteration
+ */
+ void retrieveLastLinearizedModel(StateMatrixArray& A, StateControlMatrixArray& B);
+
+ /*!
+ * the prepare Solve LQP Problem method is intended for a special use-case: unconstrained GNMS with pre-solving of the
+ */
+ virtual void prepareSolveLQProblem(size_t startIndex);
+
+
+ virtual void finishSolveLQProblem(size_t endIndex);
+
+ /*!
+ * solve Full LQProblem, e.g. to be used with HPIPM or if we have a constrained problem
+ */
+ virtual void solveFullLQProblem();
+
+ //! compute costs of solution candidate
+ void updateCosts();
+
+ //! nominal rollout using default thread and member variables for the results. // todo maybe rename (initial rollout?)
+ bool nominalRollout();
+
+ //! check problem for consistency
+ void checkProblem();
+
+ //! return the current iteration number
+ size_t& iteration();
+
+ //! Print iteration summary
+ /*!
+ * This function is automatically called if the printSummary settings is on. It prints out important information
+ * like cost etc. after each iteration.
+ */
+ void printSummary();
+
+ //! perform line-search and update controller for single shooting
+ bool lineSearchSingleShooting();
+
+ //! perform line-search and update controller for multiple shooting
+ bool lineSearchMultipleShooting();
+
+ //! build LQ approximation around trajectory (linearize dynamics and general constraints, quadratize cost)
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) = 0;
+
+ //! sets the box constraints for the entire time horizon including terminal stage
+ void setBoxConstraintsForLQOCProblem();
+
+ //! obtain state update from lqoc solver
+ void getStateUpdates();
+
+ //! obtain control update from lqoc solver
+ void getControlUpdates();
+
+ //! obtain feedback update from lqoc solver, if provided
+ void getFeedback();
+
+ //! reset all defects to zero
+ void resetDefects();
+
+ //! update the nominal defects
+ void computeDefectsNorm();
+
+ //! integrates the specified shots and computes the corresponding defects
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) = 0;
+
+ //! do a single threaded rollout and defect computation of the shots - useful for line-search
+ bool rolloutShotsSingleThreaded(size_t threadId,
+ size_t firstIndex,
+ size_t lastIndex,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateVectorArray& d,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU) const;
+
+ //! performLineSearch: execute the line search, possibly with different threading schemes
+ virtual SCALAR performLineSearch() = 0;
+
+ //! simple full-step update for state and feedforward control (used for MPC-mode!)
+ void doFullStepUpdate();
+
+ void logSummaryToMatlab(const std::string& fileName);
+
+ const SummaryAllIterations<SCALAR>& getSummary() const;
+
+protected:
+ //! integrate the individual shots
+ bool rolloutSingleShot(const size_t threadId,
+ const size_t k,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+ /*
+ bool simpleRollout(
+ const size_t threadId,
+ const ControlVectorArray& uff,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_recorded
+ )const;
+ */
+
+ //! computes the defect between shot and trajectory
+ /*!
+ * @param k index of the shot under consideration
+ * @param x_local the state trajectory
+ * @param xShot the shot trajectory
+ * @param d the defect trajectory
+ */
+ void computeSingleDefect(size_t k,
+ const StateVectorArray& x_local,
+ const StateVectorArray& xShot,
+ StateVectorArray& d) const;
+
+
+ //! Computes the linearized Dynamics at a specific point of the trajectory
+ /*!
+ This function calculates the linearization, i.e. matrices A and B in \f$ \dot{x} = A(x(k)) x + B(x(k)) u \f$
+ at a specific point of the trajectory
+
+ \param threadId the id of the worker thread
+ \param k step k
+ */
+ void computeLinearizedDynamics(size_t threadId, size_t k);
+
+
+ //! Computes the linearized general constraints at a specific point of the trajectory
+ /*!
+ This function calculates the linearization, i.e. matrices d, C and D in \f$ d_{lb} \leq C x + D u \leq d_{ub}\f$
+ at a specific point of the trajectory
+
+ \param threadId the id of the worker thread
+ \param k step k
+
+ \note the box constraints do not need to be linearized
+ */
+ void computeLinearizedConstraints(size_t threadId, size_t k);
+
+ //! Computes the quadratic costs
+ /*!
+ This function calculates the quadratic costs as provided by the costFunction pointer.
+
+ * \param threadId id of worker thread
+ * \param k step k
+ */
+ void computeQuadraticCosts(size_t threadId, size_t k);
+
+ //! Initializes cost to go
+ /*!
+ * This function initializes the cost-to-go function at time K.
+ *
+ */
+ void initializeCostToGo();
+
+ //! Computes cost to go
+ /*!
+ * This function computes the cost-to-go function for all times t<t_K
+ *
+ * \param k step k
+ */
+ void computeCostToGo(size_t k);
+
+ //! Design controller
+ /*!
+ * This function designes the LQR and feedforward controller at time k.
+ *
+ * \param k step k
+ */
+ void designController(size_t k);
+
+ //! Compute cost for a given set of state and input trajectory
+ /*!
+ * Compute cost for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param intermediateCost the accumulated intermediate cost
+ * \param finalCost the accumulated final cost
+ */
+ void computeCostsOfTrajectory(size_t threadId,
+ const core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost) const;
+
+ /*!
+ * @brief Compute box constraint violations for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param e_tot the total accumulated box constraint violation
+ */
+ void computeBoxConstraintErrorOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const;
+
+ /*!
+ * @brief Compute general constraint violations for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param e_tot the total accumulated general constraint violation
+ */
+ void computeGeneralConstraintErrorOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const;
+
+ //! Check if controller with particular alpha is better
+ void executeLineSearchSingleShooting(const size_t threadId,
+ const scalar_t alpha,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+
+ void executeLineSearchMultipleShooting(const size_t threadId,
+ const scalar_t alpha,
+ const ControlVectorArray& u_ff_update,
+ const StateVectorArray& x_update,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_recorded,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_shot_recorded,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& defects_recorded,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_recorded,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& defectNorm,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+
+ //! Update feedforward controller
+ /*!
+ * This function updates the feedforward Controller based on the previous calculation.
+ *
+ * \param k step k
+ */
+ void updateFFController(size_t k);
+
+
+ //! Send a std::vector of Eigen to Matlab
+ /*!
+ * This is a helper function to efficiently send std::vectors to Matlab.
+ */
+ // template <class V>
+ // void matrixToMatlab(V& matrix, std::string variableName);
+
+ //! compute norm of a discrete array (todo move to core)
+ template <typename ARRAY_TYPE, size_t ORDER = 1>
+ SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& d) const;
+
+ //! compute norm of difference between two discrete arrays (todo move to core)
+ template <typename ARRAY_TYPE, size_t ORDER = 1>
+ SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& a, const ARRAY_TYPE& b) const;
+
+ //! compute the norm of the defects trajectory
+ /*!
+ * Note that different kind of norms might be favorable for different cases.
+ * According to Nocedal and Wright, the l1-norm is "exact" (p.435), the l2-norm is smooth.
+ */
+ template <size_t ORDER = 1>
+ SCALAR computeDefectsNorm(const StateVectorArray& d) const;
+
+ typedef std::shared_ptr<ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>> SubstepRecorderPtr;
+ std::vector<SubstepRecorderPtr, Eigen::aligned_allocator<SubstepRecorderPtr>> substepRecorders_;
+
+ typedef std::shared_ptr<ct::core::Integrator<STATE_DIM, SCALAR>> IntegratorPtr;
+ std::vector<IntegratorPtr, Eigen::aligned_allocator<IntegratorPtr>> integrators_; //! Runge-Kutta-4 Integrators
+
+ typedef std::shared_ptr<Sensitivity_t> SensitivityPtr;
+ std::vector<SensitivityPtr, Eigen::aligned_allocator<SensitivityPtr>>
+ sensitivity_; //! the ct sensitivity integrators
+
+ typedef std::shared_ptr<ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>
+ IntegratorSymplecticEulerPtr;
+ std::vector<IntegratorSymplecticEulerPtr, Eigen::aligned_allocator<IntegratorSymplecticEulerPtr>>
+ integratorsEulerSymplectic_;
+
+ typedef std::shared_ptr<ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>
+ IntegratorSymplecticRkPtr;
+ std::vector<IntegratorSymplecticRkPtr, Eigen::aligned_allocator<IntegratorSymplecticRkPtr>>
+ integratorsRkSymplectic_;
+
+ typedef std::shared_ptr<core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>> ConstantControllerPtr;
+ std::vector<ConstantControllerPtr, Eigen::aligned_allocator<ConstantControllerPtr>>
+ controller_; //! the constant controller for forward-integration during one time-step
+
+
+ //! The policy. currently only for returning the result, should eventually replace L_ and u_ff_ (todo)
+ NLOCBackendBase::Policy_t policy_;
+
+ ct::core::tpl::TimeArray<SCALAR> t_; //! the time trajectory
+
+ bool initialized_;
+ bool configured_;
+
+ size_t iteration_; /*!< current iteration */
+
+ Settings_t settings_;
+
+ int K_; //! the number of stages in the overall OptConProblem
+
+ StateVectorArray lx_;
+ StateVectorArray x_;
+ StateVectorArray xShot_;
+ StateVectorArray x_prev_;
+
+ ControlVectorArray lu_;
+ ControlVectorArray u_ff_;
+ ControlVectorArray u_ff_prev_;
+
+ FeedbackArray L_;
+
+ SCALAR d_norm_; //! sum of the norms of all defects (internal constraint)
+ SCALAR e_box_norm_; //! sum of the norms of all box constraint violations
+ SCALAR e_gen_norm_; //! sum of the norms of all general constraint violations
+ SCALAR lx_norm_; //! sum of the norms of state update
+ SCALAR lu_norm_; //! sum of the norms of control update
+
+ //! shared pointer to the linear-quadratic optimal control problem
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>> lqocProblem_;
+
+ //! shared pointer to the linear-quadratic optimal control solver
+ std::shared_ptr<LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>> lqocSolver_;
+
+ StateSubstepsPtr substepsX_;
+ ControlSubstepsPtr substepsU_;
+
+ scalar_t intermediateCostBest_;
+ scalar_t finalCostBest_;
+ scalar_t lowestCost_;
+
+ //! costs of the previous iteration, required to determine convergence
+ scalar_t intermediateCostPrevious_;
+ scalar_t finalCostPrevious_;
+
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB
+ matlab::MatFile matFile_;
+#endif //MATLAB
+
+
+ /*!
+ * of the following objects, we have nThreads+1 instantiations in form of a vector.
+ * Every instantiation is dedicated to a certain thread in the multi-thread implementation
+ */
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systems_;
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearSystems_;
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costFunctions_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints_;
+
+
+ bool firstRollout_;
+ scalar_t alphaBest_;
+
+ //! a counter used to identify lqp problems in derived classes, i.e. for thread management in MP
+ size_t lqpCounter_;
+
+ SummaryAllIterations<SCALAR> summaryAllIterations_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <typename ARRAY_TYPE, size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDiscreteArrayNorm(
+ const ARRAY_TYPE& d) const
+{
+ SCALAR norm = 0.0;
+
+ for (size_t k = 0; k < d.size(); k++)
+ {
+ norm += d[k].template lpNorm<ORDER>();
+ }
+ return norm;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <typename ARRAY_TYPE, size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDiscreteArrayNorm(const ARRAY_TYPE& a,
+ const ARRAY_TYPE& b) const
+{
+ assert(a.size() == b.size());
+
+ SCALAR norm = 0.0;
+
+ for (size_t k = 0; k < a.size(); k++)
+ {
+ norm += (a[k] - b[k]).template lpNorm<ORDER>();
+ }
+ return norm;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDefectsNorm(
+ const StateVectorArray& d) const
+{
+ return computeDiscreteArrayNorm<StateVectorArray, ORDER>(d);
+}
+
+
+} // namespace optcon
+} // namespace ct
+
+
+#undef SYMPLECTIC_ENABLED
+#undef SYMPLECTIC_DISABLED
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
new file mode 100644
index 0000000..d9f6c71
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
@@ -0,0 +1,628 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendMP(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const NLOptConSettings& settings)
+ : Base(optConProblem, settings)
+{
+ startupRoutine();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendMP(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : Base(optConProblem, settingsFile, verbose, ns)
+{
+ startupRoutine();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendMP()
+{
+ shutdownRoutine();
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::startupRoutine()
+{
+ launchWorkerThreads();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::shutdownRoutine()
+{
+ workersActive_ = false;
+ workerTask_ = SHUTDOWN;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "Shutting down workers" << std::endl;
+#endif // DEBUG_PRINT_MP
+
+ for (size_t i = 0; i < workerThreads_.size(); i++)
+ {
+ workerThreads_[i].join();
+ }
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "All workers shut down" << std::endl;
+#endif // DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::threadWork(size_t threadId)
+{
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: launched");
+#endif // DEBUG_PRINT_MP
+
+
+ // local variables
+ int workerTask_local = IDLE;
+ size_t uniqueProcessID = 0;
+ size_t iteration_local = this->iteration_;
+ size_t lqpCounter_local = this->lqpCounter_;
+
+
+ while (workersActive_)
+ {
+ workerTask_local = workerTask_.load();
+ iteration_local = this->iteration_;
+ lqpCounter_local = this->lqpCounter_;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: previous procId: " + std::to_string(uniqueProcessID) +
+ ", current procId: " +
+ std::to_string(generateUniqueProcessID(iteration_local, (int)workerTask_local, lqpCounter_local)));
+#endif
+
+
+ /*!
+ * We want to put the worker to sleep if
+ * - the workerTask_ is IDLE
+ * - or we are finished but workerTask_ is not yet reset, thus the process ID is still the same
+ * */
+ if (workerTask_local == IDLE ||
+ uniqueProcessID == generateUniqueProcessID(iteration_local, (int)workerTask_local, lqpCounter_local))
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: going to sleep !");
+#endif
+
+ // sleep until the state is not IDLE any more and we have a different process ID than before
+ std::unique_lock<std::mutex> waitLock(workerWakeUpMutex_);
+ while (workerTask_ == IDLE || (uniqueProcessID == generateUniqueProcessID(this->iteration_,
+ (int)workerTask_.load(), this->lqpCounter_)))
+ {
+ workerWakeUpCondition_.wait(waitLock);
+ }
+ waitLock.unlock();
+
+ workerTask_local = workerTask_.load();
+ iteration_local = this->iteration_;
+ lqpCounter_local = this->lqpCounter_;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: woke up !");
+#endif // DEBUG_PRINT_MP
+ }
+
+ if (!workersActive_)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("Breaking - workers are not active !");
+#endif // DEBUG_PRINT_MP
+ break;
+ }
+
+
+ switch (workerTask_local)
+ {
+ case LINE_SEARCH:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now busy with LINE_SEARCH !");
+#endif // DEBUG_PRINT_MP
+ lineSearchWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, LINE_SEARCH, lqpCounter_local);
+ break;
+ }
+ case ROLLOUT_SHOTS:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now doing shot rollouts !");
+#endif // DEBUG_PRINT_MP
+ rolloutShotWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, ROLLOUT_SHOTS, lqpCounter_local);
+ break;
+ }
+ case COMPUTE_LQ_PROBLEM:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now doing LQ approximation !");
+#endif // DEBUG_PRINT_MP
+ computeLQProblemWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, COMPUTE_LQ_PROBLEM, lqpCounter_local);
+ break;
+ }
+ case SHUTDOWN:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now shutting down !");
+#endif // DEBUG_PRINT_MP
+ return;
+ break;
+ }
+ case IDLE:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: is idle. Now going to sleep.");
+#endif // DEBUG_PRINT_MP
+ break;
+ }
+ default:
+ {
+ printString("[Thread " + std::to_string(threadId) + "]: WARNING: Worker has unknown task !");
+ break;
+ }
+ }
+ }
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: shut down.");
+#endif // DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::launchWorkerThreads()
+{
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Going to launch worker threads!");
+ std::cout << workersActive_.load() << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ workersActive_ = true;
+ workerTask_ = IDLE;
+
+ for (int i = 0; i < (int)this->settings_.nThreads; i++)
+ {
+ workerThreads_.push_back(std::thread(&NLOCBackendMP::threadWork, this, i));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQApproximation(size_t firstIndex,
+ size_t lastIndex)
+{
+ // fill terminal cost
+ if (lastIndex == (this->K_ - 1))
+ this->initializeCostToGo();
+
+ /*
+ * In special cases, this function may be called for a single index, e.g. for the unconstrained GNMS real-time iteration scheme.
+ * Then, don't wake up workers, but do single-threaded computation for that single index, and return.
+ */
+ if (lastIndex == firstIndex)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: do single threaded LQ approximation for single index " + std::to_string(firstIndex) +
+ ". Not waking up workers.");
+#endif //DEBUG_PRINT_MP
+ this->computeLinearizedDynamics(this->settings_.nThreads, firstIndex);
+ this->computeQuadraticCosts(this->settings_.nThreads, firstIndex);
+ if (this->generalConstraints_[this->settings_.nThreads] != nullptr)
+ this->computeLinearizedConstraints(this->settings_.nThreads, firstIndex);
+ return;
+ }
+
+ /*
+ * In case of multiple points to perform LQ-approximation, start multi-threading:
+ */
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restricting Eigen to " + std::to_string(Eigen::nbThreads()) + " threads.");
+#endif //DEBUG_PRINT_MP
+
+ kTaken_ = 0;
+ kCompleted_ = 0;
+ KMax_ = lastIndex;
+ KMin_ = firstIndex;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Waking up workers to do LQ approximation.");
+#endif //DEBUG_PRINT_MP
+ workerTask_ = COMPUTE_LQ_PROBLEM;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Will sleep now until done with LQ approximation.");
+#endif //DEBUG_PRINT_MP
+
+ std::unique_lock<std::mutex> waitLock(kCompletedMutex_);
+ kCompletedCondition_.wait(waitLock, [this] { return kCompleted_.load() > KMax_ - KMin_; });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Woke up again, should have computed LQ approximation now.");
+#endif //DEBUG_PRINT_MP
+
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQProblemWorker(size_t threadId)
+{
+ while (true)
+ {
+ const size_t k = kTaken_++;
+
+ if (k > KMax_ - KMin_)
+ {
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("k > KMax_ - KMin_ with k = " + std::to_string(k) + " and KMax_ is " +
+ std::to_string(KMax_) + " and KMin_ is " + std::to_string(KMin_));
+#endif
+
+ //kCompleted_++;
+ if (kCompleted_.load() > KMax_ - KMin_)
+ kCompletedCondition_.notify_all();
+ return;
+ }
+
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("[Thread " + std::to_string(threadId) + "]: Building LQ problem for index k " +
+ std::to_string(KMax_ - k));
+#endif
+
+ this->computeQuadraticCosts(threadId, KMax_ - k); // quadratize cost backwards
+
+ this->computeLinearizedDynamics(threadId, KMax_ - k); // linearize dynamics backwards
+
+ if (this->generalConstraints_[threadId] != nullptr)
+ this->computeLinearizedConstraints(threadId, KMax_ - k); // linearize constraints backwards
+
+ kCompleted_++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShots(size_t firstIndex, size_t lastIndex)
+{
+ /*!
+ * In special cases, this function may be called for a single index, e.g. for the unconstrained GNMS real-time iteration scheme.
+ * Then, don't wake up workers, but do single-threaded computation for that single index, and return.
+ */
+ if (lastIndex == firstIndex)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: do single threaded shot rollout for single index " + std::to_string(firstIndex) +
+ ". Not waking up workers.");
+#endif //DEBUG_PRINT_MP
+
+ this->rolloutSingleShot(this->settings_.nThreads, firstIndex, this->u_ff_, this->x_, this->x_, this->xShot_,
+ *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(firstIndex, this->x_, this->xShot_, this->lqocProblem_->b_);
+ return;
+ }
+
+ /*!
+ * In case of multiple points to be linearized, start multi-threading:
+ */
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restricting Eigen to " + std::to_string(Eigen::nbThreads()) + " threads.");
+#endif //DEBUG_PRINT_MP
+
+
+ kTaken_ = 0;
+ kCompleted_ = 0;
+ KMax_ = lastIndex;
+ KMin_ = firstIndex;
+
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Waking up workers to do shot rollouts." << std::endl;
+#endif //DEBUG_PRINT_MP
+ workerTask_ = ROLLOUT_SHOTS;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Will sleep now until we have rolled out all shots." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ std::unique_lock<std::mutex> waitLock(kCompletedMutex_);
+ kCompletedCondition_.wait(waitLock, [this] { return kCompleted_.load() > KMax_ - KMin_; });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Woke up again, should have rolled out all shots now." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShotWorker(size_t threadId)
+{
+ while (true)
+ {
+ size_t k = kTaken_++;
+
+ if (k > KMax_ - KMin_)
+ {
+ if (kCompleted_.load() > KMax_ - KMin_)
+ kCompletedCondition_.notify_all();
+ return;
+ }
+
+ size_t kShot = (KMax_ - k);
+ if (kShot % ((size_t)this->settings_.K_shot) == 0) //! only rollout when we're meeting the beginning of a shot
+ {
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("[Thread " + std::to_string(threadId) + "]: rolling out shot with index " +
+ std::to_string(KMax_ - k));
+#endif
+
+ this->rolloutSingleShot(
+ threadId, kShot, this->u_ff_, this->x_, this->x_, this->xShot_, *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(kShot, this->x_, this->xShot_, this->lqocProblem_->b_);
+ }
+
+ kCompleted_++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::performLineSearch()
+{
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+
+ alphaProcessed_.clear();
+ alphaTaken_ = 0;
+ alphaBestFound_ = false;
+ alphaExpBest_ = this->settings_.lineSearchSettings.maxIterations;
+ alphaExpMax_ = this->settings_.lineSearchSettings.maxIterations;
+ alphaProcessed_.resize(this->settings_.lineSearchSettings.maxIterations, 0);
+ lowestCostPrevious_ = this->lowestCost_;
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Waking up workers." << std::endl;
+#endif //DEBUG_PRINT_MP
+ workerTask_ = LINE_SEARCH;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Will sleep now until done line search." << std::endl;
+#endif //DEBUG_PRINT_MP
+ std::unique_lock<std::mutex> waitLock(alphaBestFoundMutex_);
+ alphaBestFoundCondition_.wait(waitLock, [this] { return alphaBestFound_.load(); });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Woke up again, should have results now." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ double alphaBest = 0.0;
+ if (alphaExpBest_ != alphaExpMax_)
+ {
+ alphaBest = this->settings_.lineSearchSettings.alpha_0 *
+ std::pow(this->settings_.lineSearchSettings.n_alpha, alphaExpBest_);
+ }
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+
+ if (this->settings_.printSummary)
+ {
+ this->lu_norm_ = this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ this->u_ff_, this->u_ff_prev_);
+ this->lx_norm_ = this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ this->x_, this->x_prev_);
+ }
+ else
+ {
+#ifdef MATLAB
+ this->lu_norm_ = this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ this->u_ff_, this->u_ff_prev_);
+ this->lx_norm_ = this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ this->x_, this->x_prev_);
+#endif
+ }
+ this->x_prev_ = this->x_;
+ this->u_ff_prev_ = this->u_ff_;
+
+ return alphaBest;
+
+} // end linesearch
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchWorker(size_t threadId)
+{
+ while (true)
+ {
+ size_t alphaExp = alphaTaken_++;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: Taking alpha index " + std::to_string(alphaExp));
+#endif
+
+ if (alphaExp >= alphaExpMax_ || alphaBestFound_)
+ {
+ return;
+ }
+
+ //! convert to real alpha
+ double alpha =
+ this->settings_.lineSearchSettings.alpha_0 * std::pow(this->settings_.lineSearchSettings.n_alpha, alphaExp);
+
+ //! local variables
+ SCALAR cost = std::numeric_limits<SCALAR>::max();
+ SCALAR intermediateCost = std::numeric_limits<SCALAR>::max();
+ SCALAR finalCost = std::numeric_limits<SCALAR>::max();
+ SCALAR defectNorm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_box_norm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_gen_norm = std::numeric_limits<SCALAR>::max();
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_shot_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> defects_recorded(
+ this->K_ + 1, ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_recorded(this->K_);
+ typename Base::StateSubstepsPtr substepsX =
+ typename Base::StateSubstepsPtr(new typename Base::StateSubsteps(this->K_ + 1));
+ typename Base::ControlSubstepsPtr substepsU =
+ typename Base::ControlSubstepsPtr(new typename Base::ControlSubsteps(this->K_ + 1));
+
+ //! set init state
+ x_search[0] = this->x_prev_[0];
+
+
+ switch (this->settings_.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ {
+ this->executeLineSearchMultipleShooting(threadId, alpha, this->lu_, this->lx_, x_search, x_shot_search,
+ defects_recorded, u_recorded, intermediateCost, finalCost, defectNorm, e_box_norm, e_gen_norm,
+ *substepsX, *substepsU, &alphaBestFound_);
+ break;
+ }
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ {
+ defectNorm = 0.0;
+ this->executeLineSearchSingleShooting(threadId, alpha, x_search, u_recorded, intermediateCost,
+ finalCost, e_box_norm, e_gen_norm, *substepsX, *substepsU, &alphaBestFound_);
+ break;
+ }
+ default:
+ throw std::runtime_error("Algorithm type unknown in performLineSearch()!");
+ }
+
+ // compute merit
+ cost = intermediateCost + finalCost + this->settings_.meritFunctionRho * defectNorm +
+ this->settings_.meritFunctionRhoConstraints * (e_box_norm + e_gen_norm);
+
+ lineSearchResultMutex_.lock();
+ if (cost < lowestCostPrevious_ && !std::isnan(cost))
+ {
+ // make sure we do not alter an existing result
+ if (alphaBestFound_)
+ {
+ lineSearchResultMutex_.unlock();
+ break;
+ }
+
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ printString("[LineSearch, Thread " + std::to_string(threadId) + "]: Lower cost/merit found at alpha:" +
+ std::to_string(alpha));
+ printString("[LineSearch]: Cost:\t" + std::to_string(intermediateCost + finalCost));
+ printString("[LineSearch]: Defect:\t" + std::to_string(defectNorm));
+ printString("[LineSearch]: err box constr:\t" + std::to_string(e_box_norm));
+ printString("[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm));
+ printString("[LineSearch]: Merit:\t" + std::to_string(cost));
+ }
+
+ alphaExpBest_ = alphaExp;
+ this->intermediateCostBest_ = intermediateCost;
+ this->finalCostBest_ = finalCost;
+ this->d_norm_ = defectNorm;
+ this->e_box_norm_ = e_box_norm;
+ this->e_gen_norm_ = e_gen_norm;
+ this->lowestCost_ = cost;
+ this->x_.swap(x_search);
+ this->xShot_.swap(x_shot_search);
+ this->u_ff_.swap(u_recorded);
+ this->lqocProblem_->b_.swap(defects_recorded);
+ this->substepsX_ = substepsX;
+ this->substepsU_ = substepsU;
+ }
+ else
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ printString("[LineSearch, Thread " + std::to_string(threadId) +
+ "]: NO lower cost/merit found at alpha:" + std::to_string(alpha));
+ printString("[LineSearch]: Cost:\t" + std::to_string(intermediateCost + finalCost));
+ printString("[LineSearch]: Defect:\t" + std::to_string(defectNorm));
+ printString("[LineSearch]: err box constr:\t" + std::to_string(e_box_norm));
+ printString("[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm));
+ printString("[LineSearch]: Merit:\t" + std::to_string(cost));
+ }
+ }
+
+ alphaProcessed_[alphaExp] = 1;
+
+ // we now check if all alphas prior to the best have been processed
+ // this also covers the case that there is no better alpha
+ bool allPreviousAlphasProcessed = true;
+ for (size_t i = 0; i < alphaExpBest_; i++)
+ {
+ if (alphaProcessed_[i] != 1)
+ {
+ allPreviousAlphasProcessed = false;
+ break;
+ }
+ }
+ if (allPreviousAlphasProcessed)
+ {
+ alphaBestFound_ = true;
+ alphaBestFoundCondition_.notify_all();
+ }
+
+ lineSearchResultMutex_.unlock();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+size_t NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::generateUniqueProcessID(const size_t& iterateNo,
+ const int workerState,
+ const size_t resetCount)
+{
+ return (10e12 * (resetCount + 1) + 10e6 * (workerState + 1) + iterateNo + 1);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::printString(const std::string& text)
+{
+ std::cout << text << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp
new file mode 100644
index 0000000..fc494ea
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp
@@ -0,0 +1,138 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+
+#include "NLOCBackendBase.hpp"
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * NLOC Backend for the multi-threaded case
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendMP : public NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Base;
+
+ NLOCBackendMP(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const NLOptConSettings& settings);
+
+ NLOCBackendMP(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ //! destructor
+ virtual ~NLOCBackendMP();
+
+protected:
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) override;
+
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) override;
+
+ SCALAR performLineSearch() override;
+
+private:
+ enum WORKER_STATE
+ {
+ IDLE,
+ LINE_SEARCH,
+ ROLLOUT_SHOTS,
+ COMPUTE_LQ_PROBLEM,
+ SHUTDOWN
+ };
+
+ void startupRoutine();
+
+ void shutdownRoutine();
+
+ //! Launch all worker thread
+ /*!
+ Initializes and launches all worker threads
+ */
+ void launchWorkerThreads();
+
+ //! Main function of thread worker
+ /*!
+ Includes all tasks that a worker will execute
+ */
+ void threadWork(size_t threadId);
+
+ //! Line search for new controller using multi-threading
+ /*!
+ Line searches for the best controller in update direction. If line search is disabled, it just takes the suggested update step.
+ */
+ void lineSearchWorker(size_t threadId);
+
+ //! Creates the linear quadratic problem
+ /*!
+ This function calculates the quadratic costs as provided by the costFunction pointer as well as the linearized dynamics.
+
+ \param k step k
+ */
+ void computeLQProblemWorker(size_t threadId);
+
+ //! rolls out a shot and computes the defect
+ void rolloutShotWorker(size_t threadId);
+
+ /*! heuristic that generates a unique id for a process, such that we can manage the tasks.
+ * Generates a unique identifiers for task, iteration:
+ * @todo replace by proper hash
+ * */
+ size_t generateUniqueProcessID(const size_t& iterateNo, const int workerState, const size_t resetCount);
+
+ //! wrapper method for nice debug printing
+ void printString(const std::string& text);
+
+ std::vector<std::thread> workerThreads_;
+ std::atomic_bool workersActive_;
+ std::atomic_int workerTask_;
+
+ std::mutex workerWakeUpMutex_;
+ std::condition_variable workerWakeUpCondition_;
+
+ std::mutex kCompletedMutex_;
+ std::condition_variable kCompletedCondition_;
+
+ std::mutex kCompletedMutexCost_;
+ std::condition_variable kCompletedConditionCost_;
+
+ std::mutex lineSearchResultMutex_;
+ std::mutex alphaBestFoundMutex_;
+ std::condition_variable alphaBestFoundCondition_;
+
+ std::atomic_size_t alphaTaken_;
+ size_t alphaMax_;
+ size_t alphaExpBest_;
+ size_t alphaExpMax_;
+ std::atomic_bool alphaBestFound_;
+ std::vector<size_t> alphaProcessed_;
+
+ std::atomic_size_t kTaken_;
+ std::atomic_size_t kCompleted_;
+
+ size_t KMax_;
+ size_t KMin_;
+
+ SCALAR lowestCostPrevious_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
new file mode 100644
index 0000000..bc0eaab
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
@@ -0,0 +1,212 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendST(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const NLOptConSettings& settings)
+ : Base(optConProblem, settings)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendST(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : Base(optConProblem, settingsFile, verbose, ns)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendST(){};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQApproximation(size_t firstIndex,
+ size_t lastIndex)
+{
+ if (lastIndex == this->K_ - 1)
+ this->initializeCostToGo();
+
+ for (size_t k = firstIndex; k <= lastIndex; k++)
+ {
+ this->computeLinearizedDynamics(this->settings_.nThreads, k);
+
+ this->computeQuadraticCosts(this->settings_.nThreads, k);
+
+ if (this->generalConstraints_[this->settings_.nThreads] != nullptr)
+ this->computeLinearizedConstraints(this->settings_.nThreads, k);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShots(size_t firstIndex, size_t lastIndex)
+{
+ for (size_t k = firstIndex; k <= lastIndex; k = k + this->settings_.K_shot)
+ {
+ // rollout the shot
+ this->rolloutSingleShot(this->settings_.nThreads, k, this->u_ff_, this->x_, this->x_, this->xShot_,
+ *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(k, this->x_, this->xShot_, this->lqocProblem_->b_);
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::performLineSearch()
+{
+ // we start with extrapolation
+ double alpha = this->settings_.lineSearchSettings.alpha_0;
+ double alphaBest = 0.0;
+ size_t iterations = 0;
+
+ this->lx_norm_ = 0.0;
+ this->lu_norm_ = 0.0;
+
+
+ while (iterations < this->settings_.lineSearchSettings.maxIterations)
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ std::cout << "[LineSearch]: Iteration: " << iterations << ", try alpha: " << alpha << " out of maximum "
+ << this->settings_.lineSearchSettings.maxIterations << " iterations. " << std::endl;
+
+ iterations++;
+
+ SCALAR cost = std::numeric_limits<SCALAR>::max();
+ SCALAR intermediateCost = std::numeric_limits<SCALAR>::max();
+ SCALAR finalCost = std::numeric_limits<SCALAR>::max();
+ SCALAR defectNorm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_box_norm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_gen_norm = std::numeric_limits<SCALAR>::max();
+
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_shot_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> defects_recorded(
+ this->K_ + 1, ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_recorded(this->K_);
+
+ typename Base::StateSubstepsPtr substepsX =
+ typename Base::StateSubstepsPtr(new typename Base::StateSubsteps(this->K_ + 1));
+ typename Base::ControlSubstepsPtr substepsU =
+ typename Base::ControlSubstepsPtr(new typename Base::ControlSubsteps(this->K_ + 1));
+
+ // set init state
+ x_search[0] = this->x_[0];
+
+
+ switch (this->settings_.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ {
+ this->executeLineSearchMultipleShooting(this->settings_.nThreads, alpha, this->lu_, this->lx_, x_search,
+ x_shot_search, defects_recorded, u_recorded, intermediateCost, finalCost, defectNorm, e_box_norm,
+ e_gen_norm, *substepsX, *substepsU);
+
+ break;
+ }
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ {
+ defectNorm = 0.0;
+ this->executeLineSearchSingleShooting(this->settings_.nThreads, alpha, x_search, u_recorded,
+ intermediateCost, finalCost, e_box_norm, e_gen_norm, *substepsX, *substepsU);
+ break;
+ }
+ default:
+ throw std::runtime_error("Algorithm type unknown in performLineSearch()!");
+ }
+
+ // compute merit
+ cost = intermediateCost + finalCost + this->settings_.meritFunctionRho * defectNorm +
+ this->settings_.meritFunctionRhoConstraints * (e_box_norm + e_gen_norm);
+
+ // catch the case that a rollout might be unstable
+ if (std::isnan(cost) ||
+ cost >= this->lowestCost_) // TODO: alternatively cost >= (this->lowestCost_*(1 - 1e-3*alpha)), study this
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: No better cost/merit found at alpha " << alpha << ":" << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCost + finalCost << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << defectNorm << std::endl;
+ std::cout << "[LineSearch]: err box constr:\t" + std::to_string(e_box_norm) << std::endl;
+ std::cout << "[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm) << std::endl;
+ std::cout << "[LineSearch]: Merit:\t" << cost << std::endl;
+ }
+
+ // compute new alpha
+ alpha = alpha * this->settings_.lineSearchSettings.n_alpha;
+ }
+ else
+ {
+ // cost < this->lowestCost_ , better merit/cost found!
+
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "Lower cost/merit found at alpha: " << alpha << ":" << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCost + finalCost << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << defectNorm << std::endl;
+ std::cout << "[LineSearch]: err box constr:\t" + std::to_string(e_box_norm) << std::endl;
+ std::cout << "[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm) << std::endl;
+ std::cout << "[LineSearch]: Merit:\t" << cost << std::endl;
+ }
+
+
+ if (this->settings_.printSummary)
+ {
+ this->lu_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ u_recorded, this->u_ff_prev_);
+ this->lx_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ x_search, this->x_prev_);
+ }
+ else
+ {
+#ifdef MATLAB
+ this->lu_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ u_recorded, this->u_ff_prev_);
+ this->lx_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ x_search, this->x_prev_);
+#endif
+ }
+
+ alphaBest = alpha;
+ this->intermediateCostBest_ = intermediateCost;
+ this->finalCostBest_ = finalCost;
+ this->d_norm_ = defectNorm;
+ this->e_box_norm_ = e_box_norm;
+ this->e_gen_norm_ = e_gen_norm;
+ this->x_prev_ = x_search;
+ this->lowestCost_ = cost;
+ this->x_.swap(x_search);
+ this->xShot_.swap(x_shot_search);
+ this->u_ff_.swap(u_recorded);
+ this->lqocProblem_->b_.swap(defects_recorded);
+ this->substepsX_ = substepsX;
+ this->substepsU_ = substepsU;
+ break;
+ }
+ } // end while
+
+ return alphaBest;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp
new file mode 100644
index 0000000..d24285e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp
@@ -0,0 +1,47 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "NLOCBackendBase.hpp"
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * NLOC Backend for Single-Threaded case
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendST : public NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Base;
+
+ NLOCBackendST(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const NLOptConSettings& settings);
+
+ NLOCBackendST(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ virtual ~NLOCBackendST();
+
+protected:
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) override;
+
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) override;
+
+ SCALAR performLineSearch() override;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp
new file mode 100644
index 0000000..9ef9e26
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp
@@ -0,0 +1,96 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+/*!
+ * to do log every xth iter
+ *
+ * \todo also log settings here
+ */
+template <typename SCALAR = double>
+struct SummaryAllIterations
+{
+ //! log of the iterations
+ std::vector<size_t> iterations;
+
+ //! different overall defect norms
+ std::vector<SCALAR> defect_l1_norms;
+ std::vector<SCALAR> defect_l2_norms;
+ std::vector<SCALAR> e_box_norms;
+ std::vector<SCALAR> e_gen_norms;
+
+ //! step update norms
+ std::vector<SCALAR> lx_norms;
+ std::vector<SCALAR> lu_norms;
+
+ //! costs and merits
+ std::vector<SCALAR> intermediateCosts;
+ std::vector<SCALAR> finalCosts;
+ std::vector<SCALAR> totalCosts;
+ std::vector<SCALAR> merits;
+
+ //! step size
+ std::vector<SCALAR> stepSizes;
+
+ //! smallest eigenvalues
+ std::vector<SCALAR> smallestEigenvalues;
+
+ //! print summary of the last iteration with desired numeric precision
+ template <int NUM_PRECISION = 12>
+ void printSummaryLastIteration()
+ {
+ std::cout << "NLOC Summary of iteration " << iterations.back() << std::endl;
+ std::cout << "==============================" << std::endl;
+
+ std::cout << std::setprecision(NUM_PRECISION) << "interm. cost:\t" << intermediateCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "final cost:\t" << finalCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total cost:\t" << totalCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total merit:\t" << merits.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "tot. defect L1:\t" << defect_l1_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "tot. defect L2:\t" << defect_l2_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "box cstr.err:\t" << e_box_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "gen cstr.err:\t" << e_gen_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total lx norm:\t" << lx_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total lu norm:\t" << lu_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "step-size:\t" << stepSizes.back() << std::endl;
+ std::cout << " ===========" << std::endl;
+ std::cout << std::endl;
+ }
+
+
+ void logToMatlab(const std::string& fileName)
+ {
+#ifdef MATLAB
+ std::cout << "Logging NLOC summary to Matlab" << std::endl;
+ matFile_.open(fileName + ".mat");
+
+ matFile_.put("iterations", iterations);
+ matFile_.put("defect_l1_norms", defect_l1_norms);
+ matFile_.put("defect_l2_norms", defect_l2_norms);
+ matFile_.put("box_constr_norms", e_box_norms);
+ matFile_.put("gen_constr_norms", e_gen_norms);
+ matFile_.put("lx_norms", lx_norms);
+ matFile_.put("lu_norms", lu_norms);
+ matFile_.put("intermediateCosts", intermediateCosts);
+ matFile_.put("finalCosts", finalCosts);
+ matFile_.put("totalCosts", totalCosts);
+ matFile_.put("merits", merits);
+ matFile_.put("stepSizes", stepSizes);
+ matFile_.put("smallestEigenvalues", smallestEigenvalues);
+ matFile_.close();
+#endif
+ }
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB
+ matlab::MatFile matFile_;
+#endif //MATLAB
+};
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp
new file mode 100644
index 0000000..88994cf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp
@@ -0,0 +1,302 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::GNMS(std::shared_ptr<Backend_t>& backend_,
+ const Settings_t& settings)
+ : BASE(backend_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~GNMS()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ this->backend_->configure(settings);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ this->backend_->setInitialGuess(initialGuess);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+ prepareIteration();
+
+ return finishIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startPrepare = std::chrono::steady_clock::now();
+
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("GNMS is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("GNMS is not configured!");
+
+ this->backend_->checkProblem();
+
+ int K = this->backend_->getNumSteps();
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ this->backend_->rolloutShots(K_shot, K - 1);
+ }
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(K_shot, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: computing LQ Approximation from index " << K_shot << " to N-1 took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS]: Solving prepare stage of LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->prepareSolveLQProblem(K_shot);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Prepare phase of LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ auto endPrepare = std::chrono::steady_clock::now();
+ if (debugPrint)
+ std::cout << "[GNMS]: prepareIteration() took "
+ << std::chrono::duration<double, std::milli>(endPrepare - startPrepare).count() << " ms" << std::endl;
+
+} //! prepareIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startFinish = std::chrono::steady_clock::now();
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ this->backend_->rolloutShots(0, K_shot - 1);
+ this->backend_->updateCosts();
+ this->backend_->computeDefectsNorm();
+ }
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->computeLQApproximation(0, K_shot - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: computing LQ approximation for first multiple-shooting interval took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS]: Finish phase LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->finishSolveLQProblem(K_shot - 1);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Finish solving LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ //! update solutions
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+
+ start = std::chrono::steady_clock::now();
+ bool foundBetter = this->backend_->lineSearchMultipleShooting();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Line search took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+
+ if (debugPrint)
+ {
+ auto endFinish = std::chrono::steady_clock::now();
+ std::cout << "[GNMS]: finishIteration() took "
+ << std::chrono::duration<double, std::milli>(endFinish - startFinish).count() << " ms" << std::endl;
+ }
+
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif //MATLAB_FULL_LOG
+
+ this->backend_->iteration()++;
+
+ return foundBetter;
+
+} //! finishIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startPrepare = std::chrono::steady_clock::now();
+
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("GNMS is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("GNMS is not configured!");
+
+ this->backend_->checkProblem();
+
+ int K = this->backend_->getNumSteps();
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ this->backend_->resetDefects();
+
+ this->backend_->rolloutShots(K_shot, K - 1);
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(K_shot, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: computing LQ approximation from index " << K_shot << " to N-1 took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Solving prepare stage of LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->prepareSolveLQProblem(K_shot);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Prepare phase of LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ auto endPrepare = std::chrono::steady_clock::now();
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: prepareIteration() took "
+ << std::chrono::duration<double, std::milli>(endPrepare - startPrepare).count() << " ms" << std::endl;
+
+
+} //! prepareMPCIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startFinish = std::chrono::steady_clock::now();
+
+
+ this->backend_->rolloutShots(0, K_shot - 1);
+ this->backend_->updateCosts(); //! todo: replace by a simple sum after computeQuadraticCostsAround....
+ this->backend_->computeDefectsNorm(); //! todo: we might not need this in MPC
+
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->computeLQApproximation(0, K_shot - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: computing LQ approximation for first multiple-shooting interval took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Finish phase LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->finishSolveLQProblem(K_shot - 1);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Finish solving LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ //! update solutions
+ start = std::chrono::steady_clock::now();
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+ //!update state and controls, no line-search, overwriting happens only at next rollout
+ this->backend_->doFullStepUpdate();
+
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Solution update took " << std::chrono::duration<double, std::milli>(diff).count()
+ << " ms" << std::endl;
+
+
+ if (debugPrint)
+ {
+ auto endFinish = std::chrono::steady_clock::now();
+ std::cout << "[GNMS-MPC]: finishIteration() took "
+ << std::chrono::duration<double, std::milli>(endFinish - startFinish).count() << " ms" << std::endl;
+ }
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif //MATLAB_FULL_LOG
+
+ this->backend_->iteration()++;
+
+ return true; // note: will always return foundBetter
+
+} //! finishIteration()
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp
new file mode 100644
index 0000000..4c92f84
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp
@@ -0,0 +1,80 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class GNMS : public NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ typedef NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> BASE;
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ //! constructor
+ GNMS(std::shared_ptr<Backend_t>& backend_, const Settings_t& settings);
+
+ //! destructor
+ virtual ~GNMS();
+
+ //! configure the solver
+ virtual void configure(const Settings_t& settings) override;
+
+ //! set an initial guess
+ virtual void setInitialGuess(const Policy_t& initialGuess) override;
+
+ //! runIteration combines prepareIteration and finishIteration
+ /*!
+ * @return foundBetter (false if converged)
+ */
+ virtual bool runIteration() override;
+
+
+ /*!
+ * - linearize dynamics for the stages 1 to N-1
+ * - quadratize cost for stages 1 to N-1
+ */
+ virtual void prepareIteration() override;
+
+
+ //! finish iteration for unconstrained GNMS
+ /*!
+ * - linearize dynamcs for the first stage
+ * - quadratize cost for the first stage
+ * @return
+ */
+ virtual bool finishIteration() override;
+
+
+ //! prepare iteration, dedicated to MPC.
+ /*!
+ * requirements: no line-search, end with update-step of controls and state, no rollout after update steps.
+ * Therefore: rollout->linearize->solve
+ */
+ virtual void prepareMPCIteration() override;
+
+
+ //! finish iteration, dedicated to MPC
+ virtual bool finishMPCIteration() override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m
new file mode 100644
index 0000000..3f0c060
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m
@@ -0,0 +1,49 @@
+clear all
+close all
+load ../../../../../../../ct/GNMSLog0.mat
+
+%reformat
+t = squeeze(t);
+lv = squeeze(lv);
+
+figure()
+subplot(2,2,1)
+plot(t, x(1,:), 'kx', 'MarkerSize',1)
+hold on;
+plot(t, xShot(1,:), 'bx', 'MarkerSize',1)
+legend('x*', 'x init');
+xlabel('t [sec]')
+ylabel('x [m]')
+title('positions');
+
+subplot(2,2,2)
+plot(t, x(2,:), 'k')
+hold on;
+plot(t, xShot(2,:), 'b')
+legend('v*', 'v init');
+xlabel('t [sec]')
+ylabel('v [m/sec]')
+title('velocities');
+
+
+subplot(2,2,3)
+plot(t(1:end-1), lv)
+title('control update')
+xlabel('t [sec]')
+ylabel('F [N]')
+
+subplot(2,2,4)
+plot(t, lx(1,:)); hold on
+plot(t, lx(2,:)); hold on
+title('state update')
+xlabel('t [sec]')
+ylabel('dx [m]')
+legend('x update', 'v update')
+
+
+%%
+figure();
+plot(t(1:end-1), d(1,:)); hold on;
+%plot(t(1:end-1), d(2,:))
+title('defect')
+
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp
new file mode 100644
index 0000000..064060c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp
@@ -0,0 +1,156 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::iLQR(std::shared_ptr<Backend_t>& backend_,
+ const Settings_t& settings)
+ : BASE(backend_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~iLQR()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ this->backend_->configure(settings);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ this->backend_->setInitialGuess(initialGuess);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+ prepareIteration();
+
+ return finishIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("iLQR is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("iLQR is not configured!");
+
+ this->backend_->checkProblem();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ int K = this->backend_->getNumSteps();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ if (!this->backend_->nominalRollout())
+ throw std::runtime_error("Rollout failed. System became unstable");
+
+ this->backend_->updateCosts();
+ }
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ auto startEntire = start;
+
+ // set box constraints and do LQ approximation
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(0, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Computing LQ approximation took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ end = std::chrono::steady_clock::now();
+ diff = end - startEntire;
+ if (debugPrint)
+ std::cout << "[iLQR]: Forward pass took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ if (debugPrint)
+ std::cout << "[iLQR]: #2 Solve LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->solveFullLQProblem();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Solving LQOC problem took " << std::chrono::duration<double, std::milli>(diff).count()
+ << " ms" << std::endl;
+
+ // update solutions
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+ // line-search
+ if (debugPrint)
+ std::cout << "[iLQR]: #3 LineSearch" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ bool foundBetter = this->backend_->lineSearchSingleShooting();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Line search took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ diff = end - startEntire;
+ if (debugPrint)
+ std::cout << "[iLQR]: finishIteration took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif
+
+ this->backend_->iteration()++;
+
+ return foundBetter;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ prepareIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ finishIteration();
+ return true; //! \todo : in MPC always returning true. Unclear how user wants to deal with varying costs, etc.
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp
new file mode 100644
index 0000000..7aafca5
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp
@@ -0,0 +1,80 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class iLQR : public NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ typedef NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> BASE;
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ //! constructor
+ iLQR(std::shared_ptr<Backend_t>& backend_, const Settings_t& settings);
+
+ //! destructor
+ virtual ~iLQR();
+
+ //! configure the solver
+ virtual void configure(const Settings_t& settings) override;
+
+ //! set an initial guess
+ virtual void setInitialGuess(const Policy_t& initialGuess) override;
+
+
+ //! runIteration combines prepareIteration and finishIteration
+ /*!
+ * For iLQR the separation between prepareIteration and finishIteration would actually not be necessary
+ * @return
+ */
+ virtual bool runIteration() override;
+
+
+ /*!
+ * for iLQR, as it is a purely sequential approach, we cannot prepare anything prior to solving,
+ */
+ virtual void prepareIteration() override;
+
+
+ /*!
+ * for iLQR, finishIteration contains the whole main iLQR iteration.
+ * @return
+ */
+ virtual bool finishIteration() override;
+
+
+ /*!
+ * for iLQR, as it is a purely sequential approach, we cannot prepare anything prior to solving,
+ */
+ virtual void prepareMPCIteration() override;
+
+
+ /*!
+ * for iLQR, finishIteration contains the whole main iLQR iteration.
+ * @return
+ */
+ virtual bool finishMPCIteration() override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h
new file mode 100644
index 0000000..3591dab
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h
@@ -0,0 +1,200 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Implements an abstract base class from which all the discrete
+ * custom NLP constraints should derive
+ */
+template <typename SCALAR>
+class DiscreteConstraintBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteConstraintBase() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~DiscreteConstraintBase() {}
+ /**
+ * @brief Evaluates the constraint violation
+ *
+ * @return A vector of the evaluated constraint violation
+ */
+ virtual VectorXs eval() = 0;
+
+ /**
+ * @brief Returns the non zero elements of the eval method with respect
+ * to the optimization variables
+ *
+ * @return A vector of the non zero elements of the constraint jacobian
+ */
+ virtual VectorXs evalSparseJacobian() = 0;
+
+ /**
+ * @brief Returns size of the constraint vector
+ *
+ * @return The size of the constraint vector (should be equal to the
+ * size of the return value of the eval method)
+ */
+ virtual size_t getConstraintSize() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements of the jacobian
+ *
+ * @return The number of non zero elements of the jacobian (which should
+ * be equal to the return value of evalSparseJacobian)
+ */
+ virtual size_t getNumNonZerosJacobian() = 0;
+
+ /**
+ * @brief Returns the sparsity structure of the constraint jacobian
+ *
+ * @param[out] iRow_vec A vector containing the row indices of the non zero
+ * elements of the constraint jacobian
+ * @param[out] jCol_vec A vector containing the column indices of the non
+ * zero elements of the constraint jacobian
+ */
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) = 0;
+
+ /**
+ * @brief Returns the lower bound of the constraint
+ *
+ * @return The lower constraint bound
+ */
+ virtual VectorXs getLowerBound() = 0;
+
+ /**
+ * @brief Returns the upper bound of the constraint
+ *
+ * @return The upper constraint bound
+ */
+ virtual VectorXs getUpperBound() = 0;
+
+protected:
+ /**
+ * @brief This method generates Row and Column vectors which indicate
+ * the sparsity pattern of the constraint jacobian for a
+ * quadratic matrix block containing diagonal entries only
+ *
+ * @param[in] col_start The starting column of the jCol vec
+ * @param[in] num_elements The size of the matrix block
+ * @param[out] iRow_vec The resulting row vector
+ * @param[out] jCol_vec The resuling column vector
+ * @param[in] indexNumber The starting inserting index for iRow and jCol
+ *
+ * @return indexnumber plus num_elements
+ */
+ size_t genDiagonalIndices(const size_t col_start,
+ const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber);
+
+ /**
+ * @brief This method generates Row and Column vectors which indicate
+ * the sparsity pattern of the constraint jacobian for an
+ * arbitrary dense matrix block
+ *
+ * @param[in] col_start The starting column of the jCol vec
+ * @param[in] num_rows The number of rows of the matrix block
+ * @param[in] num_cols The number of columns of the matrix block
+ * @param[out] iRow_vec The resulting row vector
+ * @param[out] jCol_vec The resuling column vector
+ * @param[in] indexNumber The starting inserting index for iRow and jCol
+ *
+ * @return The indexnumber plus the number of elements contained in the
+ * matrix block
+ */
+ size_t genBlockIndices(const size_t col_start,
+ const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber);
+};
+
+template <typename SCALAR>
+inline size_t DiscreteConstraintBase<SCALAR>::genDiagonalIndices(const size_t col_start,
+ const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber)
+{
+ Eigen::VectorXi new_row_indices;
+ Eigen::VectorXi new_col_indices;
+ new_row_indices.resize(num_elements);
+ new_col_indices.resize(num_elements);
+
+ size_t count = 0;
+
+ for (size_t i = 0; i < num_elements; ++i)
+ {
+ new_row_indices(count) = i;
+ new_col_indices(count) = col_start + i;
+ count++;
+ }
+
+ assert(count == num_elements);
+
+ iRow_vec.segment(indexNumber, num_elements) = new_row_indices;
+ jCol_vec.segment(indexNumber, num_elements) = new_col_indices;
+
+ return count;
+}
+
+template <typename SCALAR>
+inline size_t DiscreteConstraintBase<SCALAR>::genBlockIndices(const size_t col_start,
+ const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber)
+{
+ size_t num_gen_indices = num_rows * num_cols;
+
+ Eigen::VectorXi new_row_indices;
+ Eigen::VectorXi new_col_indices;
+ new_row_indices.resize(num_gen_indices);
+ new_col_indices.resize(num_gen_indices);
+
+ size_t count = 0;
+
+ for (size_t row = 0; row < num_rows; ++row)
+ {
+ for (size_t col = col_start; col < col_start + num_cols; ++col)
+ {
+ new_row_indices(count) = row;
+ new_col_indices(count) = col;
+ count++;
+ }
+ }
+
+ assert(count == num_gen_indices);
+
+ iRow_vec.segment(indexNumber, num_gen_indices) = new_row_indices;
+ jCol_vec.segment(indexNumber, num_gen_indices) = new_col_indices;
+
+ return num_gen_indices;
+}
+}
+
+typedef tpl::DiscreteConstraintBase<double> DiscreteConstraintBase;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h
new file mode 100644
index 0000000..d288b4e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h
@@ -0,0 +1,201 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+#include "DiscreteConstraintBase.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief An abstract base class which serves as a container for all the
+ * discrete constraints used in the NLP
+ */
+template <typename SCALAR>
+class DiscreteConstraintContainerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteConstraintContainerBase() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~DiscreteConstraintContainerBase() {}
+ /**
+ * @brief Gets called before the constraint evaluation. This method
+ * should contain all the calculations needed to evaluate the
+ * constraints
+ */
+ virtual void prepareEvaluation() = 0;
+
+ /**
+ * @brief Gets called before the constraint jacobian evaluation. This
+ * method should contain all the calculations needed to evaluate
+ * the constraint jacobian
+ */
+ virtual void prepareJacobianEvaluation() = 0;
+
+
+ /**
+ * @brief Writes the constraint evaluations into the large constraint
+ * optimization vector
+ *
+ * @param[out] c_nlp The constraint vector used in the NLP
+ */
+ void evalConstraints(MapVecXs& c_nlp)
+ {
+ prepareEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ c_nlp.segment(ind, cSize) = constraint->eval();
+ ind += cSize;
+ }
+
+ assert(ind == c_nlp.rows()); // or throw an error
+ }
+
+ void evalConstraints(VectorXs& c_nlp)
+ {
+ prepareEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ c_nlp.segment(ind, cSize) = constraint->eval();
+ ind += cSize;
+ }
+
+ assert(static_cast<int>(ind) == c_nlp.rows()); // or throw an error
+ }
+
+ /**
+ * @brief Evaluates the jacobian of the constraints and writes them
+ * into the nlp vector
+ *
+ * @param[out] jac_nlp The constraint jacobian vector used in NLP
+ * @param[in] nzz_jac_g The number of non zero elements in the jacobian
+ */
+ void evalSparseJacobian(MapVecXs& jac_nlp, const int nzz_jac_g)
+ {
+ prepareJacobianEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t nnEle = constraint->getNumNonZerosJacobian();
+ jac_nlp.segment(ind, nnEle) = constraint->evalSparseJacobian();
+ ind += nnEle;
+ }
+
+ assert(static_cast<int>(ind) == nzz_jac_g);
+ }
+
+ /**
+ * @brief Retrieves the sparsity pattern of the constraints and writes
+ * them into the nlp vectors
+ *
+ * @param[out] iRow_nlp The vector containing the row indices of the non
+ * zero entries of the constraint jacobian
+ * @param[out] jCol_nlp The vector containing the column indices of the
+ * non zero entries of the constraint jacobian
+ * @param[in] nnz_jac_g The number of non zero elements in the constraint jacobian
+ */
+ void getSparsityPattern(Eigen::Map<Eigen::VectorXi>& iRow_nlp,
+ Eigen::Map<Eigen::VectorXi>& jCol_nlp,
+ const int nnz_jac_g)
+ {
+ size_t ind = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t nnEle = constraint->getNumNonZerosJacobian();
+ size_t cSize = constraint->getConstraintSize();
+ Eigen::VectorXi iRow, jCol;
+ iRow.resize(nnEle);
+ jCol.resize(nnEle);
+ constraint->genSparsityPattern(iRow, jCol);
+ iRow_nlp.segment(ind, nnEle) = iRow.array() + constraintCount;
+ jCol_nlp.segment(ind, nnEle) = jCol;
+ constraintCount += cSize;
+ ind += nnEle;
+ }
+
+ assert(static_cast<int>(ind) == nnz_jac_g);
+ }
+
+ /**
+ * @brief Returns the number of constraints in the NLP
+ *
+ * @return The number of constraint in the NLP
+ */
+ size_t getConstraintsCount() const
+ {
+ size_t count = 0;
+ for (auto constraint : constraints_)
+ count += constraint->getConstraintSize();
+ return count;
+ }
+
+ /**
+ * @brief Returns the number of non zeros in the constraint jacobian
+ *
+ * @return The number of non zeros in the constraint jacobian
+ */
+ size_t getNonZerosJacobianCount() const
+ {
+ size_t count = 0;
+ for (auto constraint : constraints_)
+ count += constraint->getNumNonZerosJacobian();
+ return count;
+ }
+
+ /**
+ * @brief Retrieves the constraint bounds and writes them into the
+ * vectors used in the NLP
+ *
+ * @param[out] lowerBound The lower constraint bound
+ * @param[out] upperBound The lower constraint bound
+ */
+ void getBounds(MapVecXs& lowerBound, MapVecXs& upperBound)
+ {
+ size_t ind = 0;
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ lowerBound.segment(ind, cSize) = constraint->getLowerBound();
+ upperBound.segment(ind, cSize) = constraint->getUpperBound();
+ ind += cSize;
+ }
+ }
+
+protected:
+ std::vector<std::shared_ptr<DiscreteConstraintBase<SCALAR>>>
+ constraints_; /*!< contains all the constraints of the NLP */
+};
+}
+
+typedef tpl::DiscreteConstraintContainerBase<double> DiscreteConstraintContainerBase;
+}
+}
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h
new file mode 100644
index 0000000..1fc0cb6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h
@@ -0,0 +1,57 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Implements an abstract base class which evaluates the cost
+ * function and its gradient in the NLP
+ */
+template <typename SCALAR>
+class DiscreteCostEvaluatorBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteCostEvaluatorBase(){};
+
+ /**
+ * @brief Destructor.
+ */
+ virtual ~DiscreteCostEvaluatorBase(){};
+
+
+ /**
+ * @brief Evaluates the cost function
+ *
+ * @return The evaluates cost function
+ */
+ virtual SCALAR eval() = 0;
+
+ /**
+ * @brief Evaluates the cost gradient
+ *
+ * @param[in] grad_length The size of the gradient vector
+ * @param[out] grad The values of the gradient
+ */
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) = 0;
+};
+}
+
+typedef tpl::DiscreteCostEvaluatorBase<double> DiscreteCostEvaluatorBase;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/Nlp b/ct_optcon/include/ct/optcon/nlp/Nlp
new file mode 100644
index 0000000..0c2c7b8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/Nlp
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "Nlp.h"
+#include "solver/NlpSolver.h"
+#include "solver/IpoptSolver.h"
+#include "solver/SnoptSolver.h"
+#include "solver/NlpSolverSettings.h"
+
diff --git a/ct_optcon/include/ct/optcon/nlp/Nlp.h b/ct_optcon/include/ct/optcon/nlp/Nlp.h
new file mode 100644
index 0000000..7442050
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/Nlp.h
@@ -0,0 +1,493 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <Eigen/Sparse>
+#include <ct/core/math/DerivativesCppadJIT.h>
+#include "OptVector.h"
+#include "DiscreteConstraintContainerBase.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+/** @defgroup NLP NLP
+ *
+ * @brief The nonlinear program module
+ */
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The NLP base class. This class serves as abstract base class to
+ * use as an interface to the NLP solver IPOPT and SNOPT
+ */
+template <typename SCALAR>
+class Nlp
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ /**
+ * @brief Default constructor
+ */
+ Nlp() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~Nlp() {}
+ /**
+ * @brief { This method gets called at each update of the Optimization
+ * variables. This can be used to distribute or rearrange the
+ * optimization variables appropriately }
+ */
+ virtual void updateProblem() = 0;
+
+ /**
+ * @brief { Evaluates the costfunction at the current nlp iteration }
+ *
+ * @return { Scalar value of the resulting cost }
+ */
+ SCALAR evaluateCostFun()
+ {
+ if (!costCodegen_ && !costEvaluator_)
+ throw std::runtime_error("Error in evaluateCostFun. Costevaluator not initialized");
+
+ if (costCodegen_)
+ return costCodegen_->forwardZero(optVariables_->getOptimizationVars())(0);
+ else
+ return costEvaluator_->eval();
+ }
+
+
+ /**
+ * @brief { Evaluates the gradient of the costfunction}
+ *
+ * @param[in] n { size of the gradient }
+ * @param[out] grad The gradient of the cost function
+ */
+ void evaluateCostGradient(const size_t n, MapVecXs& grad)
+ {
+ if (!costCodegen_ && !costEvaluator_)
+ throw std::runtime_error("Error in evaluateCostGradient. Costevaluator not initialized");
+
+ if (costCodegen_)
+ grad = costCodegen_->jacobian(optVariables_->getOptimizationVars());
+ else
+ costEvaluator_->evalGradient(n, grad);
+ }
+
+ /**
+ * @brief { Evaluates the constraints }
+ *
+ * @param[out] values The values of the constraint violations, wrapped as a
+ * vector
+ */
+ void evaluateConstraints(MapVecXs& values)
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in evaluateConstraints. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ values = constraintsCodegen_->forwardZero(optVariables_->getOptimizationVars());
+ else
+ constraints_->evalConstraints(values);
+ }
+
+ /**
+ * @brief { Evaluates the constraint jacobian }
+ *
+ * @param[in] nele_jac The number of non zeros in the constraint jacobian
+ * @param[out] jac The non zero values of the jacobian
+ */
+ void evaluateConstraintJacobian(const int nele_jac, MapVecXs& jac)
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in evaluateConstraintJacobian. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ jac = constraintsCodegen_->sparseJacobianValues(optVariables_->getOptimizationVars());
+ else
+ constraints_->evalSparseJacobian(jac, nele_jac);
+ }
+
+ /**
+ * @brief Evaluates the hessian of the lagrangian
+ *
+ * @param[in] nele_hes The number of non zeros in the hessian
+ * @param hes The values of the non-zeros of the hessian
+ * @param[in] obj_fac The costfunction multiplier
+ * @param lambda The constraint multipliers
+ */
+ void evaluateHessian(const int nele_hes, MapVecXs& hes, const SCALAR obj_fac, MapConstVecXs& lambda)
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ throw std::runtime_error(
+ "Error in evaluateHessian. Hessian Evaluation only implemented for codegeneration");
+
+ hes.setZero();
+ Eigen::Matrix<double, 1, 1> omega;
+ omega << obj_fac;
+
+ Eigen::VectorXd hessianCostValues =
+ costCodegen_->sparseHessianValues(optVariables_->getOptimizationVars(), omega);
+ Eigen::VectorXd hessianConstraintsValues =
+ constraintsCodegen_->sparseHessianValues(optVariables_->getOptimizationVars(), lambda);
+
+ for (size_t i = 0; i < hessianCostValues.rows(); ++i)
+ hessianCost_.coeffRef(iRowHessianCost_(i), jColHessianCost_(i)) = hessianCostValues(i);
+
+ for (size_t i = 0; i < hessianConstraintsValues.rows(); ++i)
+ hessianConstraints_.coeffRef(iRowHessianConstraints_(i), jColHessianConstraints_(i)) =
+ hessianConstraintsValues(i);
+
+ hessianTotal_ = (hessianCost_ + hessianConstraints_).template triangularView<Eigen::Lower>();
+
+ hes = Eigen::Map<Eigen::VectorXd>(hessianTotal_.valuePtr(), nele_hes, 1);
+ }
+
+ /**
+ * @brief Gets the sparsity pattern.
+ *
+ * @param[in] nele_jac The number of non zero elements in the constraint
+ * jacobian
+ * @param[out] iRow The row indices of the location of the non zero
+ * elements of the constraint jacobian
+ * @param[out] jCol The column indices of the location of the non zero
+ * elements of the constraint jacobian
+ */
+ void getSparsityPatternJacobian(const int nele_jac, MapVecXi& iRow, MapVecXi& jCol) const
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in getSparsityPatternJacobian. Constraints not initialized");
+
+ iRow.setZero();
+ jCol.setZero();
+
+ if (constraintsCodegen_)
+ {
+ Eigen::VectorXi iRow1;
+ Eigen::VectorXi jCol1;
+ constraintsCodegen_->getSparsityPatternJacobian(iRow1, jCol1);
+
+ iRow = iRow1;
+ jCol = jCol1;
+ }
+ else
+ constraints_->getSparsityPattern(iRow, jCol, nele_jac);
+ }
+
+ /**
+ * @brief Gets the sparsity pattern hessian of the lagrangian
+ *
+ * @param[in] nele_hes The number of non zero elements in the hessian
+ * @param iRow The row indices
+ * @param jCol The column indices
+ */
+ void getSparsityPatternHessian(const int nele_hes, MapVecXi& iRow, MapVecXi& jCol) const
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ throw std::runtime_error(
+ "Error in getSparsityPatternHessian. Hessian Evaluation only implemented for codegeneration");
+
+ iRow.setZero();
+ jCol.setZero();
+
+ iRow = iRowHessian_;
+ jCol = jColHessian_;
+ }
+
+ /**
+ * @brief Returns the number of constraints in the NLP
+ *
+ * @return The number of constraints.
+ */
+ size_t getConstraintsCount() const
+ {
+ if (!constraints_)
+ throw std::runtime_error("Error in getConstraintsCount. Constraints not initialized");
+
+ return constraints_->getConstraintsCount();
+ }
+
+ /**
+ * @brief Returns the number of the non zero elements of the constraint
+ * jacobian.
+ *
+ * @return The number of the non zero elements of the constraint
+ * jacobian.
+ */
+ size_t getNonZeroJacobianCount() const
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in getNonZeroJacobianCount. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ return constraintsCodegen_->getNumNonZerosJacobian();
+ else
+ return constraints_->getNonZerosJacobianCount();
+ }
+
+ /**
+ * @brief Returns the number of non zeros in the hessian
+ *
+ * @return The number of non zeros in the hessian
+ */
+ size_t getNonZeroHessianCount()
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ {
+ std::cerr << "Error: Exact hessians only work with AD hessian codegeneration" << std::endl;
+ throw std::runtime_error("Error in getNonZeroHessianCount. Codegeneration not initialized");
+ }
+
+ costCodegen_->getSparsityPatternHessian(iRowHessianCost_, jColHessianCost_);
+ constraintsCodegen_->getSparsityPatternHessian(iRowHessianConstraints_, jColHessianConstraints_);
+
+ std::vector<Eigen::Triplet<SCALAR>> tripletsCost;
+ std::vector<Eigen::Triplet<SCALAR>> tripletsConstraints;
+
+ for (size_t i = 0; i < iRowHessianCost_.rows(); ++i)
+ tripletsCost.push_back(Eigen::Triplet<SCALAR>(iRowHessianCost_(i), jColHessianCost_(i), SCALAR(0.1)));
+
+ for (size_t i = 0; i < iRowHessianConstraints_.rows(); ++i)
+ tripletsConstraints.push_back(
+ Eigen::Triplet<SCALAR>(iRowHessianConstraints_(i), jColHessianConstraints_(i), SCALAR(0.1)));
+
+ hessianCost_.resize(optVariables_->size(), optVariables_->size());
+ hessianCost_.setFromTriplets(tripletsCost.begin(), tripletsCost.end());
+ hessianConstraints_.resize(optVariables_->size(), optVariables_->size());
+ hessianConstraints_.setFromTriplets(tripletsConstraints.begin(), tripletsConstraints.end());
+
+ hessianTotal_.resize(optVariables_->size(), optVariables_->size());
+ hessianTotal_ = (hessianCost_ + hessianConstraints_).template triangularView<Eigen::Lower>();
+
+ std::vector<int> iRowHessianStdVec;
+ std::vector<int> jColHessianStdVec;
+ for (size_t k = 0; k < hessianTotal_.outerSize(); ++k)
+ for (typename Eigen::SparseMatrix<SCALAR>::InnerIterator it(hessianTotal_, k); it; ++it)
+ {
+ iRowHessianStdVec.push_back(it.row());
+ jColHessianStdVec.push_back(it.col());
+ }
+
+ iRowHessian_ = Eigen::Map<Eigen::VectorXi>(iRowHessianStdVec.data(), iRowHessianStdVec.size(), 1);
+ jColHessian_ = Eigen::Map<Eigen::VectorXi>(jColHessianStdVec.data(), jColHessianStdVec.size(), 1);
+
+ size_t nonZerosHessian = iRowHessian_.rows();
+ return nonZerosHessian;
+ }
+
+ /**
+ * @brief Reads the bounds of the constraints.
+ *
+ * @param[out] lowerBound The lower constraint bound
+ * @param[out] upperBound The upper constraint bound
+ * @param[in] m { The size of the constraints }
+ */
+ void getConstraintBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t m) const
+ {
+ if (!constraints_)
+ throw std::runtime_error("Error in getConstraintBounds. Constraints not initialized");
+
+ constraints_->getBounds(lowerBound, upperBound);
+ }
+
+ /**
+ * @brief Returns the number of Optimization optimization variables
+ *
+ * @return The number of Optimization variables.
+ */
+ size_t getVarCount() const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getVarCount. Optvariables not initialized");
+
+ return optVariables_->size();
+ }
+
+ /**
+ * @brief Reads the bounds on the Optimization optimization variables.
+ *
+ * @param[out] lowerBound The lower optimization variable bound
+ * @param[out] upperBound The upper optimization variable bound
+ * @param[in] n { The number of Optimization variables }
+ */
+ void getVariableBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t n) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getVariableBounds. Optvariables not initialized");
+
+ optVariables_->getLowerBounds(lowerBound);
+ optVariables_->getUpperBounds(upperBound);
+ }
+
+ /**
+ * @brief {Extracts the Optimization optimization variables from the nlp
+ * solvers between nlp iterations}
+ *
+ * @param[in] x { The value of the Optimization variables }
+ * @param[in] isNew Indicates whether x differs from a previous call
+ */
+ void extractOptimizationVars(const MapConstVecXs& x, bool isNew)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractOptimizationVars. Optvariables not initialized");
+
+ if (isNew)
+ {
+ optVariables_->setOptimizationVars(x);
+ updateProblem();
+ }
+ }
+
+ /**
+ * @brief Gets the Optimization variables.
+ *
+ * @param[in] n { The number of Optimization variables }
+ * @param[out] x { The values of the Optimization vars }
+ */
+ void getInitialGuess(const size_t n, MapVecXs& x) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getOptimizationVars. Optvariables not initialized");
+
+ optVariables_->getInitialGuess(n, x);
+ }
+
+ /**
+ * @brief Gets the variable multiplier and the variable state, used in
+ * the NLP solver SNOPT. See the snopt documentation for further
+ * explanations
+ *
+ * @param[in] n { The number of Optimization variables }
+ * @param[out] xMul The optimization variable multiplier
+ * @param[out] xState The optimization variable states
+ */
+ void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getOptimizationMultState. Optvariables not initialized");
+
+ optVariables_->getOptimizationMultState(n, xMul, xState);
+ }
+
+ /**
+ * @brief Gets the constraint multiplier and state, used in the NLP
+ * solver SNOPT.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] zMul The constraint variable multiplier
+ * @param[out] zState The constraint variable state
+ */
+ void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getConstraintsMultState. Optvariables not initialized");
+
+ optVariables_->getConstraintsMultState(m, zMul, zState);
+ }
+
+ /**
+ * @brief Gets the bound multipliers used in the NLP solver IPOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] zLow The value for the lower bound multiplier
+ * @param[out] zUp The value for the upper bound multiplier
+ */
+ void getBoundMultipliers(size_t n, MapVecXs& zLow, MapVecXs& zUp) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getBoundMultipliers. Optvariables not initialized");
+
+ optVariables_->getBoundMultipliers(n, zLow, zUp);
+ }
+
+ /**
+ * @brief Gets the values of the constraint multipliers.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] lambda The values of the constraint multipliers
+ */
+ void getLambdaVars(size_t m, MapVecXs& lambda) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getLambdaVars. Optvariables not initialized");
+
+ optVariables_->getLambdaVars(m, lambda);
+ }
+
+ /**
+ * @brief { Extracts the solution values from IPOPT }
+ *
+ * @param[in] x { The values of the optimization variables }
+ * @param[in] zL The value for the lower bound multiplier
+ * @param[in] zU The value for the upper bound multiplier
+ * @param[in] lambda The values of the constraint multipliers
+ */
+ void extractIpoptSolution(const MapConstVecXs& x,
+ const MapConstVecXs& zL,
+ const MapConstVecXs& zU,
+ const MapConstVecXs& lambda)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractIpoptSolution. Optvariables not initialized");
+
+ optVariables_->setNewIpoptSolution(x, zL, zU, lambda);
+ }
+
+ /**
+ * @brief { Extracts the solution values from SNOPT }
+ *
+ * @param[in] x { The values of the optimization variables }
+ * @param[in] xMul The optimization variable multiplier
+ * @param[in] xState The optimization variable state
+ * @param[in] fMul The constraint variable multiplier
+ * @param[in] fState The constraint variable state
+ */
+ void extractSnoptSolution(const MapVecXs& x,
+ const MapVecXs& xMul,
+ const MapVecXi& xState,
+ const MapVecXs& fMul,
+ const MapVecXi& fState)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractSnoptSolution. Optvariables not initialized");
+
+ optVariables_->setNewSnoptSolution(x, xMul, xState, fMul, fState);
+ }
+
+
+protected:
+ std::shared_ptr<DiscreteCostEvaluatorBase<SCALAR>>
+ costEvaluator_; //! abstract base class, approximates the cost evaluation for the discrete problem
+ std::shared_ptr<OptVector<SCALAR>>
+ optVariables_; //! base class, containts the optimization variables used in the NLP solvers
+ std::shared_ptr<DiscreteConstraintContainerBase<SCALAR>>
+ constraints_; //! abstract base class, contains the discretized constraints for the problem
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, 1>> costCodegen_;
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, -1>> constraintsCodegen_;
+
+ Eigen::SparseMatrix<SCALAR> hessianCost_, hessianConstraints_, hessianTotal_;
+
+ Eigen::VectorXi iRowHessianCost_, iRowHessianConstraints_, jColHessianCost_, jColHessianConstraints_;
+ Eigen::VectorXi iRowHessian_;
+ Eigen::VectorXi jColHessian_;
+};
+}
+
+typedef tpl::Nlp<double> Nlp;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/OptVector.h b/ct_optcon/include/ct/optcon/nlp/OptVector.h
new file mode 100644
index 0000000..d2c4dbc
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/OptVector.h
@@ -0,0 +1,322 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Class containing and managing all the optimization variables used
+ * for in the NLP solver IPOPT and SNOPT.
+ */
+template <typename SCALAR>
+class OptVector
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ OptVector() = delete;
+
+ /**
+ * @brief { Constructor resizing the vectors of the optimization
+ * variables to the correct size }
+ *
+ * @param[in] n The number of the optimization variables
+ */
+ OptVector(const size_t n) : updateCount_(0)
+ {
+ x_.resize(n);
+ x_.setZero();
+ xInit_.resize(n);
+ xLb_.resize(n);
+ xUb_.resize(n);
+ xLb_.setConstant(std::numeric_limits<SCALAR>::lowest());
+ xUb_.setConstant(std::numeric_limits<SCALAR>::max());
+ xMul_.resize(n);
+ xMul_.setZero();
+ xState_.resize(n);
+ xState_.setZero();
+ zUpper_.resize(n);
+ zUpper_.setZero();
+ zLow_.resize(n);
+ zLow_.setZero();
+ }
+
+ /**
+ * @brief Resizes the vectors of the constraint variables to the
+ * correct size
+ *
+ * @param[in] m The number of constraints
+ */
+ void resizeConstraintVars(size_t m)
+ {
+ zMul_.resize(m + 1);
+ zMul_.setZero();
+ zState_.resize(m + 1);
+ zState_.setZero();
+ }
+
+ void reset() { updateCount_ = 0; }
+ /**
+ * @brief Destructor
+ */
+ virtual ~OptVector() {}
+ /**
+ * @brief Resizes the vectors of the optimization variables
+ *
+ * @param[in] size The size of the new optimization variables
+ */
+ void resizeOptimizationVars(const size_t size)
+ {
+ x_.resize(size);
+ xInit_.resize(size);
+ xLb_.resize(size);
+ xUb_.resize(size);
+ lambda_.resize(size);
+ zUpper_.resize(size);
+ zLow_.resize(size);
+ }
+
+
+ /**
+ * @brief Resets the optimization variables
+ */
+ void setZero()
+ {
+ x_.setZero();
+ xInit_.setZero();
+ xLb_.setZero();
+ xUb_.setZero();
+ lambda_.setZero();
+ zUpper_.setZero();
+ zLow_.setZero();
+ }
+
+ /**
+ * @brief Checks if the optimization variables have to correct size
+ *
+ * @param[in] n The number of optimization variables
+ *
+ * @return returns true of the dimensions match
+ */
+ bool checkOptimizationVarDimension(const unsigned int n)
+ {
+ bool xDim = x_.size() == n ? true : false;
+ bool xLDim = xLb_.size() == n ? true : false;
+ bool xUDim = xUb_.size() == n ? true : false;
+ return xDim && xLDim && xUDim;
+ }
+
+ /**
+ * @brief Sets the optimization variable bounds.
+ *
+ * @param[in] xLb The lower optimization variable bound
+ * @param[in] xUb The upper optimization variable bound
+ */
+ void setBounds(const VectorXs& xLb, const VectorXs& xUb)
+ {
+ xLb_ = xLb;
+ xUb_ = xUb;
+ }
+
+
+ /**
+ * @brief Gets the lower bounds of the optimization variables.
+ *
+ * @param[out] x Lower bound
+ */
+ void getLowerBounds(MapVecXs& x) const { x = xLb_; }
+ /**
+ * @brief Gets the upper bounds of the optimization variables.
+ *
+ * @param[out] x The upper bound
+ */
+ void getUpperBounds(MapVecXs& x) const { x = xUb_; }
+ /**
+ * @brief Return the state and the multiplier of the optimization
+ * variables, used in the NLP solver SNOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] xMul The optimization variables multiplier
+ * @param[out] xState The optimization variables state
+ */
+ void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
+ {
+ assert(n == xMul_.size());
+ assert(n == xState_.size());
+ xMul = xMul_;
+ xState = xState_;
+ }
+
+ /**
+ * @brief Gets the constraint multiplier and state, used in the NLP
+ * solver SNOPT.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] zMul The constraint variable multiplier
+ * @param[out] zState The constraint variable state
+ */
+ void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
+ {
+ assert(m == zMul_.size());
+ assert(m == zState_.size());
+ zMul = zMul_;
+ zState = zState_;
+ }
+
+ /**
+ * @brief Returns the number of optimization variables
+ *
+ * @return the number of optimization variables
+ */
+ size_t size() const { return x_.size(); }
+ /**
+ * @brief Gets the bound multipliers used in the NLP solver IPOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] low The value for the lower bound multiplier
+ * @param[out] up The value for the upper bound multiplier
+ */
+ void getBoundMultipliers(size_t n, MapVecXs& low, MapVecXs& up) const
+ {
+ assert(n == zLow_.size());
+ low = zLow_;
+ up = zUpper_;
+ }
+
+ /**
+ * @brief Gets the values of the constraint multipliers.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] x The values of the constraint multipliers
+ */
+ void getLambdaVars(size_t m, MapVecXs& x) const
+ {
+ assert(m == lambda_.size());
+ x = lambda_;
+ }
+
+ /**
+ * @brief Gets the optimization variables.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] x The optimization variables
+ */
+ void getOptimizationVars(size_t n, MapVecXs& x) const
+ {
+ assert(n == x_.size());
+ x = x_;
+ }
+
+ VectorXs getOptimizationVars() const { return x_; }
+ void getInitialGuess(size_t n, MapVecXs& x) const
+ {
+ assert(n == xInit_.size());
+ x = xInit_;
+ }
+
+ /**
+ * @brief Extracts the solution from ipopt and stores them into class
+ * variables
+ *
+ * @param[in] x The optimization variables
+ * @param[in] zL The lower bound multiplier
+ * @param[in] zU The upper bound multiplier
+ * @param[in] lambda The constraint multiplier
+ */
+ void setNewIpoptSolution(const MapConstVecXs& x,
+ const MapConstVecXs& zL,
+ const MapConstVecXs& zU,
+ const MapConstVecXs& lambda)
+ {
+ x_ = x;
+ zLow_ = zL;
+ zUpper_ = zU;
+ lambda_ = lambda;
+ }
+
+ /**
+ * @brief Extracts the solution from snopt and stores it into class
+ * variables
+ *
+ * @param[in] x The optimization variables
+ * @param[in] xMul The optimization variables multiplier
+ * @param[in] xState The optimization variables state
+ * @param[in] fMul The constraints multiplier
+ * @param[in] fState The constraints state
+ */
+ void setNewSnoptSolution(const MapVecXs& x,
+ const MapVecXs& xMul,
+ const MapVecXi& xState,
+ const MapVecXs& fMul,
+ const MapVecXi& fState)
+ {
+ x_ = x;
+ xMul_ = xMul;
+ xState_ = xState;
+ zMul_ = fMul;
+ zState_ = fState;
+ }
+
+ /**
+ * @brief Sets the updates optimization variables from the NLP solver
+ * and updates the counter
+ *
+ * @param[in] x The updates primal variables
+ */
+ void setOptimizationVars(const MapConstVecXs& x)
+ {
+ x_ = x;
+ updateCount_++;
+ }
+
+ void setOptimizationVars(const VectorXs& x)
+ {
+ x_ = x;
+ updateCount_++;
+ }
+
+
+ /**
+ * @brief Returns the update counter
+ *
+ * @return The update counter
+ */
+ size_t getUpdateCount() const { return updateCount_; }
+protected:
+ VectorXs x_; /*!< The optimization variables */
+ VectorXs xInit_;
+ VectorXs xLb_; /*!< lower bound on optimization vector */
+ VectorXs xUb_; /*!< upper bound on optimization vector */
+
+ VectorXs zUpper_; /*!< The upper bound multiplier, used in IPOPT */
+ VectorXs zLow_; /*!< The lower bound multiplier, used in IPOPT */
+ VectorXs lambda_; /*!< The constraint multiplier, used in IPOPT */
+
+ // Snopt variables
+ VectorXs xMul_; /*!< The optimization variable multiplier, used in SNOPT */
+ VectorXi xState_; /*!< The optimization variable state, used in SNOPT */
+ VectorXs zMul_; /*!< The constraint multiplier, used in SNOPT */
+ VectorXi zState_; /*!< The constraint state, used in SNOPT */
+
+ size_t updateCount_; /*!< The number of optimization variable updates */
+};
+}
+
+typedef tpl::OptVector<double> OptVector;
+}
+}
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h
new file mode 100644
index 0000000..2ec1366
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolver.h"
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT // build IPOPT interface
+
+#include "IpTNLP.hpp"
+#include "IpIpoptApplication.hpp"
+#include "IpSolveStatistics.hpp"
+
+#endif // BUILD_WITH_IPOPT_SUPPORT
+
+// #define DEBUG_PRINT
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT // build IPOPT interface
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The interface to the NLP solver IPOPT
+ *
+ * For the implementation see ct/ct_optcon/src/nlp/solver/IpoptSolver.cpp
+ */
+template <typename SCALAR>
+class IpoptSolver : public Ipopt::TNLP, public NlpSolver<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef NlpSolver<SCALAR> BASE;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings The nlp settings
+ */
+ IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, const NlpSolverSettings& settings);
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~IpoptSolver();
+
+ virtual bool solve() override;
+
+ virtual void prepareWarmStart(size_t maxIterations) override;
+
+ virtual void configureDerived(const NlpSolverSettings& settings) override;
+
+ /**@name Overloaded from TNLP */
+ //@{
+ /** Method to return some info about the nlp */
+ virtual bool get_nlp_info(Ipopt::Index& n,
+ Ipopt::Index& m,
+ Ipopt::Index& nnz_jac_g,
+ Ipopt::Index& nnz_h_lag,
+ IndexStyleEnum& index_style);
+
+ /** Method to return the bounds for my problem */
+ virtual bool get_bounds_info(Ipopt::Index n, SCALAR* x_l, SCALAR* x_u, Ipopt::Index m, SCALAR* g_l, SCALAR* g_u);
+
+ /** Method to return the starting point for the algorithm */
+ virtual bool get_starting_point(Ipopt::Index n,
+ bool init_x,
+ SCALAR* x,
+ bool init_z,
+ SCALAR* z_L,
+ SCALAR* z_U,
+ Ipopt::Index m,
+ bool init_lambda,
+ SCALAR* lambda);
+
+ /** Method to return the objective value */
+ virtual bool eval_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR& obj_value);
+
+ /** Method to return the gradient of the objective */
+ virtual bool eval_grad_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR* grad_f);
+
+ /** Method to return the constraint residuals */
+ virtual bool eval_g(Ipopt::Index n, const SCALAR* x, bool new_x, Ipopt::Index m, SCALAR* g);
+
+ /** Method to return:
+ * 1) The structure of the jacobian (if "values" is NULL)
+ * 2) The values of the jacobian (if "values" is not NULL)
+ */
+ virtual bool eval_jac_g(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ Ipopt::Index m,
+ Ipopt::Index nele_jac,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values);
+
+ /** Method to return:
+ * 1) The structure of the hessian of the lagrangian (if "values" is NULL)
+ * 2) The values of the hessian of the lagrangian (if "values" is not NULL)
+ */
+ virtual bool eval_h(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ SCALAR obj_factor,
+ Ipopt::Index m,
+ const SCALAR* lambda,
+ bool new_lambda,
+ Ipopt::Index nele_hess,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values);
+
+ //@}
+
+ /** @name Solution Methods */
+ //@{
+ /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
+ virtual void finalize_solution(Ipopt::SolverReturn status,
+ Ipopt::Index n,
+ const SCALAR* x,
+ const SCALAR* z_L,
+ const SCALAR* z_U,
+ Ipopt::Index m,
+ const SCALAR* g,
+ const SCALAR* lambda,
+ SCALAR obj_value,
+ const Ipopt::IpoptData* ip_data,
+ Ipopt::IpoptCalculatedQuantities* ip_cq);
+ //@}
+
+private:
+ /**
+ * @brief Sets the IPOPT solver options.
+ */
+ void setSolverOptions();
+ std::shared_ptr<Ipopt::IpoptApplication> ipoptApp_; /*!< A pointer to ipopt*/
+ Ipopt::ApplicationReturnStatus status_; /*!< The return status of IPOPT*/
+ IpoptSettings settings_; /*!< Contains the IPOPT settings*/
+
+ /**@name Methods to block default compiler methods.
+ * The compiler automatically generates the following three methods.
+ * Since the default compiler implementation is generally not what
+ * you want (for all but the most simple classes), we usually
+ * put the declarations of these methods in the private section
+ * and never implement them. This prevents the compiler from
+ * implementing an incorrect "default" behavior without us
+ * knowing. (See Scott Meyers book, "Effective C++")
+ *
+ */
+ //@{
+ IpoptSolver(const IpoptSolver&);
+ IpoptSolver& operator=(const IpoptSolver&);
+ //@}
+};
+
+#include "implementation/IpoptSolver-impl.h"
+
+#else // BUILD_WITH_IPOPT_SUPPORT -- not building with IPOPT support, create dummy class
+
+template <typename SCALAR>
+class IpoptSolver : public NlpSolver<SCALAR>
+{
+public:
+ IpoptSolver() { throw(std::runtime_error("Error - IPOPT interface not compiled.")); }
+ IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, NlpSolverSettings settings)
+ {
+ throw(std::runtime_error("Error - IPOPT interface not compiled."));
+ }
+
+ virtual bool solve() override { return false; }
+ virtual void prepareWarmStart(size_t maxIterations) override {}
+ virtual void configureDerived(const NlpSolverSettings& settings) override {}
+};
+
+#endif // BUILD_WITH_IPOPT_SUPPORT
+ //
+} // namespace tpl
+
+typedef tpl::IpoptSolver<double> IpoptSolver;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h
new file mode 100644
index 0000000..3c01e00
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h
@@ -0,0 +1,97 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolverSettings.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Abstract base class for the NLP solvers
+ */
+template <typename SCALAR>
+class NlpSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ NlpSolver() : isInitialized_(false) {}
+ /**
+ * @brief Custom Constructor 1
+ *
+ * @param[in] nlp The nlp
+ */
+ NlpSolver(std::shared_ptr<Nlp<SCALAR>> nlp) : nlp_(nlp) {}
+ /**
+ * @brief Custom constructor 2
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings Custom user settings
+ */
+ NlpSolver(std::shared_ptr<Nlp<SCALAR>> nlp, NlpSolverSettings settings)
+ : nlp_(nlp), settings_(settings), isInitialized_(false)
+ {
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~NlpSolver() {}
+ /**
+ * @brief Configures the solver with new settings
+ *
+ * @param[in] settings The nlp solver settings
+ */
+ void configure(const NlpSolverSettings& settings)
+ {
+ settings_ = settings;
+ configureDerived(settings);
+ }
+
+ /**
+ * @brief Forwards the settings to the corresponding nlp solver
+ *
+ * @param[in] settings The nlp settings
+ */
+ virtual void configureDerived(const NlpSolverSettings& settings) = 0;
+
+ /**
+ * @brief Solves the nlp
+ *
+ * @return Returns true of solve succeeded
+ */
+ virtual bool solve() = 0;
+
+ /**
+ * @brief Prepares the solver for a warmstarting scenario with
+ * available (good) initial guess
+ *
+ * @param[in] maxIterations Specifies the maximum number of nlp iteration
+ * the user is willing to spend
+ */
+ virtual void prepareWarmStart(const size_t maxIterations) = 0;
+
+ bool isInitialized() { return isInitialized_; }
+protected:
+ std::shared_ptr<Nlp<SCALAR>> nlp_; /*!< The non linear program*/
+ NlpSolverSettings settings_; /*!< The nlp settings */
+ bool isInitialized_; /*!< Indicates whether the solver is initialized */
+};
+}
+
+typedef tpl::NlpSolver<double> NlpSolver;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h
new file mode 100644
index 0000000..48f4072
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h
@@ -0,0 +1,327 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief SnoptSolver settings. Details about the parameters can be found
+ * in the SNOPT documentation
+ */
+class SnoptSettings
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * @brief Default constructor, sets the parameters to default values
+ */
+ SnoptSettings()
+ : scale_option_param_(0),
+ derivative_option_param_(1),
+ verify_level_param_(-1),
+ major_iteration_limit_param_(500),
+ minor_iteration_limit_param_(500),
+ iteration_limit_param_(5000),
+ major_optimality_tolerance_param_(1e-6),
+ major_feasibility_tolerance_param_(1e-6),
+ minor_feasibility_tolerance_param_(1e-6),
+ print_file_param_(0),
+ minor_print_level_param_(0),
+ major_print_level_param_(0),
+ new_basis_file_param_(0),
+ old_basis_file_param_(0),
+ backup_basis_file_param_(0),
+ line_search_tolerance_param_(0.9),
+ crash_option_(3),
+ hessian_updates_(5)
+ {
+ }
+
+ int scale_option_param_;
+ int derivative_option_param_;
+ int verify_level_param_;
+ int major_iteration_limit_param_;
+ int minor_iteration_limit_param_;
+ int iteration_limit_param_;
+ double major_optimality_tolerance_param_;
+ double major_feasibility_tolerance_param_;
+ double minor_feasibility_tolerance_param_;
+ int print_file_param_;
+ int minor_print_level_param_;
+ int major_print_level_param_;
+ int new_basis_file_param_;
+ int old_basis_file_param_;
+ int backup_basis_file_param_;
+ double line_search_tolerance_param_;
+ int crash_option_;
+ int hessian_updates_;
+
+ /**
+ * @brief Prints out information about the settings
+ */
+ void print()
+ {
+ std::cout << "SNOPT settings:" << std::endl;
+ std::cout << "Max Major Iterations: " << major_iteration_limit_param_ << std::endl;
+ std::cout << "Max Minor Iterations: " << minor_iteration_limit_param_ << std::endl;
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const { return true; }
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info file
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms.solver.snopt")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ minor_iteration_limit_param_ = pt.get<unsigned int>(ns + ".MaxMinorIterations");
+ major_iteration_limit_param_ = pt.get<unsigned int>(ns + ".MaxMajorIterations");
+ minor_print_level_param_ = pt.get<unsigned int>(ns + ".MinorPrintLevelVerbosity");
+ major_print_level_param_ = pt.get<unsigned int>(ns + ".MajorPrintLevelVerbosity");
+ major_optimality_tolerance_param_ = pt.get<double>(ns + ".OptimalityTolerance");
+ }
+};
+
+/**
+ * @ingroup NLP
+ *
+ * @brief IPOPT settings. Details about the parameters can be found
+ * in the IPOPT documentation
+ */
+class IpoptSettings
+{
+public:
+ /**
+ * @brief Default constructor, sets the parameters to default values
+ */
+ IpoptSettings()
+ : tol_(1e-8),
+ constr_viol_tol_(1e-4),
+ max_iter_(200),
+ restoTol_(1e-7),
+ acceptableTol_(1e-6),
+ restoAcceptableTol_(1e-7),
+ linear_scaling_on_demand_("yes"),
+ hessian_approximation_("limited-memory"),
+ nlp_scaling_method_("gradient-based"),
+ printLevel_(5),
+ print_user_options_("no"),
+ print_frequency_iter_(1),
+ printInfoString_("no"),
+ derivativeTest_("none"),
+ derivativeTestTol_(1e-4),
+ derivativeTestPerturbation_(1e-8),
+ point_perturbation_radius_(10),
+ checkDerivativesForNaninf_("no"),
+ derivativeTestPrintAll_("no"),
+ linearSystemScaling_("mc19"),
+ linear_solver_("ma57"),
+ jacobianApproximation_("finite-difference-values")
+ {
+ }
+
+ double tol_;
+ double constr_viol_tol_;
+ int max_iter_;
+ double restoTol_;
+ double acceptableTol_;
+ double restoAcceptableTol_;
+ std::string linear_scaling_on_demand_;
+ std::string hessian_approximation_;
+ std::string nlp_scaling_method_;
+ int printLevel_;
+ std::string print_user_options_;
+ int print_frequency_iter_;
+ std::string printInfoString_;
+ std::string derivativeTest_;
+ double derivativeTestTol_;
+ double derivativeTestPerturbation_;
+ double point_perturbation_radius_;
+ std::string checkDerivativesForNaninf_;
+ std::string derivativeTestPrintAll_;
+ std::string linearSystemScaling_;
+ std::string linear_solver_;
+ std::string jacobianApproximation_;
+
+ /**
+ * @brief Prints out information about the settings
+ */
+ void print()
+ {
+ std::cout << "IPOPT SETTINGS: " << std::endl;
+ std::cout << "Using " << hessian_approximation_ << " hessian approximation" << std::endl;
+ std::cout << "MaxIterations: " << max_iter_ << std::endl;
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const { return true; }
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info fil
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms.nlp.ipopt")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt); //
+ max_iter_ = pt.get<unsigned int>(ns + ".MaxIterations");
+ bool checkDerivatives = pt.get<bool>(ns + ".CheckDerivatives");
+ if (checkDerivatives)
+ derivativeTest_ = "first-order";
+ if (!checkDerivatives)
+ derivativeTest_ = "none";
+ bool exactHessian = pt.get<bool>(ns + ".ExactHessian");
+ if (exactHessian)
+ hessian_approximation_ = "exact";
+ if (!exactHessian)
+ hessian_approximation_ = "limited-memory";
+
+ printLevel_ = pt.get<unsigned int>(ns + ".Verbosity");
+ tol_ = pt.get<double>(ns + ".OptimalityTolerance");
+ }
+};
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Contains the NLP solver settings
+ */
+class NlpSolverSettings
+{
+public:
+ typedef enum SolverType { IPOPT = 0, SNOPT = 1, num_types_solver } SolverType_t;
+
+ /**
+ * @brief Default constructor, set default settings
+ */
+ NlpSolverSettings() : solverType_(IPOPT), useGeneratedCostGradient_(false), useGeneratedConstraintJacobian_(false)
+ {
+ }
+
+ SolverType_t solverType_;
+ bool useGeneratedCostGradient_;
+ bool useGeneratedConstraintJacobian_;
+ SnoptSettings snoptSettings_;
+ IpoptSettings ipoptSettings_;
+
+ /**
+ * @brief Prints out settings
+ */
+ void print()
+ {
+ std::cout << "=============================================================" << std::endl;
+ std::cout << "\tNLP Solver Settings: " << std::endl;
+ std::cout << "=============================================================" << std::endl;
+
+ std::cout << "Using nlp solver: " << solverToString[solverType_] << std::endl;
+ if (useGeneratedCostGradient_)
+ std::cout << "Using generated Cost Gradient" << std::endl;
+ else
+ std::cout << "Using analytical cost Gradient" << std::endl;
+ if (useGeneratedConstraintJacobian_)
+ std::cout << "Using generated Constraints Jacobian" << std::endl;
+ else
+ std::cout << "Using anlyitical Constraints Jacobian" << std::endl;
+
+ if (solverType_ == IPOPT)
+ ipoptSettings_.print();
+ else if (solverType_ == SNOPT)
+ snoptSettings_.print();
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const
+ {
+ if (solverType_ == IPOPT)
+ return ipoptSettings_.parametersOk();
+ else if (solverType_ == SNOPT)
+ return snoptSettings_.parametersOk();
+ else
+ return false;
+ }
+
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info fil
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "solver")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ solverType_ = (SolverType)pt.get<unsigned int>(ns + ".SolverType");
+ useGeneratedCostGradient_ = pt.get<bool>(ns + ".UseGeneratedCostGradient");
+ useGeneratedConstraintJacobian_ = pt.get<bool>(ns + ".UseGeneratedConstraintJacobian");
+
+ if (solverType_ == IPOPT)
+ ipoptSettings_.load(filename, verbose, ns + ".ipopt");
+ else if (solverType_ == SNOPT)
+ snoptSettings_.load(filename, verbose, ns + ".snopt");
+
+ if (verbose)
+ {
+ std::cout << "Loaded NLP Solver settings from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+private:
+ std::map<SolverType, std::string> solverToString = {{IPOPT, "IPOPT"}, {SNOPT, "SNOPT"}};
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h
new file mode 100644
index 0000000..fb73350
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h
@@ -0,0 +1,314 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+
+#pragma once
+
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolver.h"
+
+#ifdef BUILD_WITH_SNOPT_SUPPORT // build SNOPT interface
+#include <snoptProblem.hpp>
+#endif //BUILD_WITH_SNOPT_SUPPORT
+
+namespace ct {
+namespace optcon {
+
+#ifdef BUILD_WITH_SNOPT_SUPPORT // build SNOPT interface
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Forward declaration
+ */
+class SnoptSolver;
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Contains all the dynamically allocated memory for SNOPT
+ *
+ * For the implementation see ct/ct_optcon/src/nlp/solver/SnoptSolver.cpp
+ *
+ */
+struct SnoptMemory
+{
+ typedef double Number;
+ const SnoptSolver &self; /*!<A reference the the Snoptsolver the memory points to*/
+
+
+ Number *x_ = nullptr; /*!<Optimization variables*/
+ Number *xlow_ = nullptr; /*!<Lower bound of the optimization variables*/
+ Number *xupp_ = nullptr; /*!<Upper bound of the optimization variables*/
+ Number *xmul_ = nullptr; /*!<The optimization variables multiplier*/
+ int *xstate_ = nullptr; //*!<The state of the optimization variables*/
+
+ Number *F_ = nullptr; /*!<Nonlinear parts of the costfunction and the constraints*/
+ Number *Flow_ = nullptr; /*!<Lower bound on F*/
+ Number *Fupp_ = nullptr; /*!<Upper bound on F*/
+ Number *Fmul_ = nullptr; /*!<The F multiplier*/
+ int *Fstate_ = nullptr; /*!<The F state*/
+
+ Number *A_ = nullptr; /*!<Contains the linear parts of costfunction and the constraints*/
+ int *iAfun_ = nullptr; /*!<Rows of the sparsity pattern of A*/
+ int *jAvar_ = nullptr; /*!<Columns of the sparsity pattern of A*/
+
+ /*!<The sparsity pattern of the costgradient and constraint jacobian*/
+ int *iGfun_ = nullptr; /*!<Sparsity rows*/
+ int *jGvar_ = nullptr; /*!<Sparsity columns*/
+
+ static std::vector<SnoptMemory *> mempool; /*!<Containts all the instances of the snopt memory blocks*/
+ int memind; /*!<The index inside the mempool this instance points to*/
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] self A reference to the Snoptsolver the memory is pointing
+ * to
+ */
+ SnoptMemory(const SnoptSolver &self);
+
+ /// Destructor
+ ~SnoptMemory();
+};
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The interface to the NLP solver SNOPT. Currently the SnoptA C++
+ * interface is implemented, which does not require the distinction
+ * between linear and non linear parts of the userfunction
+ */
+class SnoptSolver : public NlpSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef double Number;
+ typedef NlpSolver BASE;
+ typedef Eigen::Matrix<Number, Eigen::Dynamic, 1> VectorXd;
+ typedef Eigen::Map<VectorXd> MapVecXd;
+ typedef Eigen::Map<const VectorXd> MapConstVecXd;
+ typedef Eigen::Map<Eigen::VectorXi> MapVecXi;
+
+ /**
+ * @brief Default constructor
+ */
+ SnoptSolver() {}
+ /**
+ * @brief Destructor, releases the memory
+ */
+ virtual ~SnoptSolver();
+ // {
+ // free_memory(memoryPtr_);
+ // }
+
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings The settings
+ */
+ SnoptSolver(std::shared_ptr<Nlp> nlp, const NlpSolverSettings &settings);
+
+
+ /**
+ * @brief The non static NLP function interfacing with the optimization
+ * module of the control toolbox
+ *
+ * @param m A pointer to the dynamic SNOPT memory
+ * @param Status Indicates the first and last call to the method
+ * @param n The number of optimization variables
+ * @param x The optimization variables
+ * @param needF If true, the method needs to provide f
+ * @param neF The length of the F vector
+ * @param F The F vector, containing the costfunction and the
+ * constraints
+ * @param needG If true, the method needs to provide g
+ * @param neG The length of the G vector
+ * @param G The vector of derivatives of F with respect to the
+ * decision variables
+ * @param cu The character array for external user inputs
+ * @param lencu The length of cu
+ * @param iu The integer array for external user inputs
+ * @param leniu The length of ui
+ * @param ru The real array for external user inputs
+ * @param lenru The length of ru
+ */
+ void NLP_Function(SnoptMemory *m,
+ int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru) const;
+
+ /**
+ * @brief The static NLP function passed to SNOPT
+ *
+ * @param Status Indicates the first and last call to the method
+ * @param n The number of optimization variables
+ * @param x The optimization variables
+ * @param needF If true, the method needs to provide f
+ * @param neF The length of the F vector
+ * @param F The F vector, containing the costfunction and the
+ * constraints
+ * @param needG If true, the method needs to provide g
+ * @param neG The length of the G vector
+ * @param G The vector of derivatives of F with respect to the
+ * decision variables
+ * @param cu The character array for external user inputs
+ * @param lencu The length of cu
+ * @param iu The integer array for external user inputs
+ * @param leniu The length of ui
+ * @param ru The real array for external user inputs
+ * @param lenru The length of ru
+ */
+ static void NLP_Function(int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru);
+
+ virtual void configureDerived(const NlpSolverSettings &settings) override;
+
+ virtual bool solve() override;
+
+ virtual void prepareWarmStart(size_t maxIterations) override;
+
+
+private:
+ /**
+ * @brief Allocates memory for the SNOPT interface
+ *
+ * @return Pointer to the memory location
+ */
+ SnoptMemory *alloc_memory() const { return new SnoptMemory(*this); }
+ /**
+ * @brief Frees the allocated memory
+ *
+ * @param[in] mem The memory to be freed
+ */
+ inline void free_memory(SnoptMemory *mem) const { delete mem; }
+ /**
+ * @brief Initializes the memory
+ *
+ * @param mem The memory
+ */
+ void init_memory(SnoptMemory *mem) const;
+
+ /**
+ * @brief Fills the memory with values from NLP, gets called before
+ * solving the NLP
+ *
+ * @param mem The memory
+ */
+ void fill_memory(SnoptMemory *mem) const;
+
+ /**
+ * @brief Provides SNOPT the information from SNOPT memory, gets called
+ * before solving the NLP
+ */
+ void setupSnoptObjects();
+
+ /**
+ * @brief Prints out status messages depending on the snopt status
+ *
+ * @param[in] status The SNOPT solver status
+ */
+ void validateSNOPTStatus(const int &status) const;
+
+ /**
+ * @brief Sets the solver options.
+ */
+ void setSolverOptions();
+
+ SnoptSettings settings_;
+ SnoptMemory *memoryPtr_;
+
+ snoptProblemA snoptApp_;
+
+ int n_ = 0; /*!<Number of optimization variables in optimization problem*/
+ int neF_ = 0; /*!<Number of constraints in optimization problem plus 1 (objective function belongs to constraints)*/
+ int ObjRow_ = 0; /*!<Objective function row*/
+ Number ObjAdd_ = 0.0; /*!<Constant to be added to objective function*/
+
+ const int Cold_ = 0, Basis_ = 1, Warm_ = 2; /*!<Defines the warmstart options for SNOPT*/
+ int currStartOption_ = Cold_; /*!<The option handed to SNOPT*/
+
+
+ int lenA_ =
+ 0; /*!<The dimension of the sparsity pattern for the linear part of the problem. We set it to zero to treat all parts nonlinearly*/
+ int neA_ = 0; /*!<The number of non zeros in A. We set it to zero to treat all parts nonlinearly*/
+
+ int lenG_ = 0; /*!<The dimension of the sparsity pattern for the linear part of the problem. Will be updated later*/
+ int neG_ = 0; /*!<The number of non zeros in G. Will be updated later*/
+
+ int status_ = 0; /*!<The exit status of the SNOPT solver*/
+};
+
+
+#else // BUILD_WITH_SNOPT_SUPPORT -- not building with SNOPT support, create dummy class
+
+class SnoptSolver : public NlpSolver // <STATE_DIM, CONTROL_DIM>
+{
+public:
+ SnoptSolver() { throw(std::runtime_error("Error - SNOPT interface not compiled.")); }
+ SnoptSolver(std::shared_ptr<Nlp> nlp, NlpSolverSettings settings)
+ {
+ throw(std::runtime_error("Error - SNOPT interface not compiled."));
+ }
+
+ virtual void configureDerived(const NlpSolverSettings& settings) override {}
+ virtual bool solve() override { return false; }
+ virtual void prepareWarmStart(size_t maxIterations) override {}
+};
+
+#endif // BUILD_WITH_SNOPT_SUPPORT
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h b/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h
new file mode 100644
index 0000000..45e65ad
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h
@@ -0,0 +1,386 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+
+// #define DEBUG_PRINT
+
+
+template <typename SCALAR>
+IpoptSolver<SCALAR>::IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, const NlpSolverSettings& settings)
+ : BASE(nlp, settings), settings_(BASE::settings_.ipoptSettings_)
+{
+ //Constructor arguments
+ //Argument 1: create console output
+ //Argument 2: create empty
+ ipoptApp_ = std::shared_ptr<Ipopt::IpoptApplication>(new Ipopt::IpoptApplication(true, false));
+}
+
+template <typename SCALAR>
+IpoptSolver<SCALAR>::~IpoptSolver()
+{
+ // Needed to destruct all the IPOPT memory
+ Ipopt::Referencer* t = NULL;
+ this->ReleaseRef(t);
+ delete t;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::configureDerived(const NlpSolverSettings& settings)
+{
+ std::cout << "calling Ipopt configure derived" << std::endl;
+ settings_ = settings.ipoptSettings_;
+ setSolverOptions();
+ this->isInitialized_ = true;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::setSolverOptions()
+{
+ ipoptApp_->Options()->SetNumericValue("tol", settings_.tol_);
+ ipoptApp_->Options()->SetNumericValue("constr_viol_tol", settings_.constr_viol_tol_);
+ ipoptApp_->Options()->SetIntegerValue("max_iter", settings_.max_iter_);
+ // ipoptApp_->Options()->SetNumericValue("resto.tol", settings_.restoTol_);
+ // ipoptApp_->Options()->SetNumericValue("acceptable_tol", settings_.acceptableTol_);
+ // ipoptApp_->Options()->SetNumericValue("resto.acceptable_tol", settings_.restoAcceptableTol_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_scaling_on_demand", settings_.linear_scaling_on_demand_);
+ ipoptApp_->Options()->SetStringValueIfUnset("hessian_approximation", settings_.hessian_approximation_);
+ // ipoptApp_->Options()->SetStringValueIfUnset("nlp_scaling_method", settings_.nlp_scaling_method_);
+ ipoptApp_->Options()->SetIntegerValue("print_level", settings_.printLevel_); //working now
+ ipoptApp_->Options()->SetStringValueIfUnset("print_user_options", settings_.print_user_options_);
+ // ipoptApp_->Options()->SetIntegerValue("print_frequency_iter", settings_.print_frequency_iter_);
+ ipoptApp_->Options()->SetStringValueIfUnset("derivative_test", settings_.derivativeTest_);
+ ipoptApp_->Options()->SetIntegerValue("print_level", settings_.printLevel_);
+ ipoptApp_->Options()->SetNumericValue("derivative_test_tol", settings_.derivativeTestTol_);
+ ipoptApp_->Options()->SetNumericValue("derivative_test_perturbation", settings_.derivativeTestPerturbation_);
+ ipoptApp_->Options()->SetNumericValue("point_perturbation_radius", settings_.point_perturbation_radius_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_system_scaling", settings_.linearSystemScaling_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_solver", settings_.linear_solver_);
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::solve()
+{
+ status_ = ipoptApp_->Initialize();
+ if (!(status_ == Ipopt::Solve_Succeeded) && !this->isInitialized_)
+ throw(std::runtime_error("NLP initialization failed"));
+
+ // Ask Ipopt to solve the problem
+ status_ = ipoptApp_->OptimizeTNLP(this);
+
+ if (status_ == Ipopt::Solve_Succeeded || status_ == Ipopt::Solved_To_Acceptable_Level)
+ {
+ // Retrieve some statistics about the solve
+ Ipopt::Index iter_count = ipoptApp_->Statistics()->IterationCount();
+ std::cout << std::endl
+ << std::endl
+ << "*** The problem solved in " << iter_count << " iterations!" << std::endl;
+
+ SCALAR final_obj = ipoptApp_->Statistics()->FinalObjective();
+ std::cout << std::endl
+ << std::endl
+ << "*** The final value of the objective function is " << final_obj << '.' << std::endl;
+ return true;
+ }
+ else
+ {
+ std::cout << " ipopt return value: " << status_ << std::endl;
+ return false;
+ }
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::prepareWarmStart(size_t maxIterations)
+{
+ ipoptApp_->Options()->SetStringValue("warm_start_init_point", "yes");
+ ipoptApp_->Options()->SetNumericValue("warm_start_bound_push", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_bound_frac", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_slack_bound_frac", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_slack_bound_push", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_mult_bound_push", 1e-9);
+ ipoptApp_->Options()->SetIntegerValue("max_iter", (int)maxIterations);
+ ipoptApp_->Options()->SetStringValue("derivative_test", "none");
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_nlp_info(Ipopt::Index& n,
+ Ipopt::Index& m,
+ Ipopt::Index& nnz_jac_g,
+ Ipopt::Index& nnz_h_lag,
+ IndexStyleEnum& index_style)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_nlp_info()" << std::endl;
+#endif
+ n = this->nlp_->getVarCount();
+ assert(n == n);
+
+ // std::cout << 1 << std::endl;
+
+ m = this->nlp_->getConstraintsCount();
+ assert(m == m);
+
+ nnz_jac_g = this->nlp_->getNonZeroJacobianCount();
+ assert(nnz_jac_g == nnz_jac_g);
+
+ if (settings_.hessian_approximation_ == "exact")
+ nnz_h_lag = this->nlp_->getNonZeroHessianCount();
+
+ index_style = Ipopt::TNLP::C_STYLE;
+
+#ifdef DEBUG_PRINT
+ std::cout << "... number of decision variables = " << n << std::endl;
+ std::cout << "... number of constraints = " << m << std::endl;
+ std::cout << "... nonzeros in jacobian = " << nnz_jac_g << std::endl;
+#endif
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_bounds_info(Ipopt::Index n,
+ SCALAR* x_l,
+ SCALAR* x_u,
+ Ipopt::Index m,
+ SCALAR* g_l,
+ SCALAR* g_u)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_bounds_info()" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs x_lVec(x_l, n);
+ MapVecXs x_uVec(x_u, n);
+ MapVecXs g_lVec(g_l, m);
+ MapVecXs g_uVec(g_u, m);
+ this->nlp_->getVariableBounds(x_lVec, x_uVec, n);
+ // bounds on optimization vector
+ // x_l <= x <= x_u
+ assert(x_l == x_l);
+ assert(x_u == x_u);
+ assert(n == n);
+
+ // constraints bounds (e.g. for equality constraints = 0)
+ this->nlp_->getConstraintBounds(g_lVec, g_uVec, m);
+ assert(g_l == g_l);
+ assert(g_u == g_u);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... Leaving get_bounds_info()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_starting_point(Ipopt::Index n,
+ bool init_x,
+ SCALAR* x,
+ bool init_z,
+ SCALAR* z_L,
+ SCALAR* z_U,
+ Ipopt::Index m,
+ bool init_lambda,
+ SCALAR* lambda)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_starting_point()" << std::endl;
+#endif //DEBUG_PRINT
+ //
+ if (init_x)
+ {
+ MapVecXs xVec(x, n);
+ this->nlp_->getInitialGuess(n, xVec);
+ }
+
+ if (init_z)
+ {
+ MapVecXs z_lVec(z_L, n);
+ MapVecXs z_uVec(z_U, n);
+ this->nlp_->getBoundMultipliers(n, z_lVec, z_uVec);
+ }
+
+ if (init_lambda)
+ {
+ MapVecXs lambdaVec(lambda, m);
+ this->nlp_->getLambdaVars(m, lambdaVec);
+ }
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_starting_point()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR& obj_value)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_f()" << std::endl;
+#endif //DEBUG_PRINT
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ obj_value = this->nlp_->evaluateCostFun();
+ assert(obj_value == obj_value);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_f()" << std::endl;
+#endif //DEBUG_PRINT
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_grad_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR* grad_f)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_grad_f()" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs grad_fVec(grad_f, n);
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateCostGradient(n, grad_fVec);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_grad_f()" << std::endl;
+#endif //DEBUG_PRINT
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_g(Ipopt::Index n, const SCALAR* x, bool new_x, Ipopt::Index m, SCALAR* g)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_g()" << std::endl;
+#endif //DEBUG_PRINT
+ assert(m == this->nlp_->getConstraintsCount());
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ MapVecXs gVec(g, m);
+ this->nlp_->evaluateConstraints(gVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "gVec: " << gVec.transpose() << std::endl;
+ std::cout << "... leaving eval_g()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_jac_g(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ Ipopt::Index m,
+ Ipopt::Index nele_jac,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values)
+{
+ if (values == NULL)
+ {
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_jac_g, values == NULL" << std::endl;
+#endif //DEBUG_PRINT
+ // set indices of nonzero elements of the jacobian
+ Eigen::Map<Eigen::VectorXi> iRowVec(iRow, nele_jac);
+ Eigen::Map<Eigen::VectorXi> jColVec(jCol, nele_jac);
+ this->nlp_->getSparsityPatternJacobian(nele_jac, iRowVec, jColVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_jac_g, values == NULL" << std::endl;
+#endif //DEBUG_PRINT
+ }
+ else
+ {
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_jac_g, values != NULL" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs valVec(values, nele_jac);
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateConstraintJacobian(nele_jac, valVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_jac_g, values != NULL" << std::endl;
+#endif //DEBUG_PRINT
+ }
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_h(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ SCALAR obj_factor,
+ Ipopt::Index m,
+ const SCALAR* lambda,
+ bool new_lambda,
+ Ipopt::Index nele_hess,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_h()" << std::endl;
+#endif
+ if (values == NULL)
+ {
+ // return the structure. This is a symmetric matrix, fill the lower left
+ // triangle only.
+ Eigen::Map<Eigen::VectorXi> iRowVec(iRow, nele_hess);
+ Eigen::Map<Eigen::VectorXi> jColVec(jCol, nele_hess);
+ this->nlp_->getSparsityPatternHessian(nele_hess, iRowVec, jColVec);
+ }
+ else
+ {
+ // return the values. This is a symmetric matrix, fill the lower left
+ // triangle only
+ MapVecXs valVec(values, nele_hess);
+ MapConstVecXs xVec(x, n);
+ MapConstVecXs lambdaVec(lambda, m);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateHessian(nele_hess, valVec, obj_factor, lambdaVec);
+ }
+
+// only needed if quasi-newton approximation is not used, hence set to -1 (not used)!
+// ATTENTION: for hard coding of the hessian, one only needs the lower left corner (since it is symmetric) - IPOPT knows that
+//nnz_h_lag = -1;
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_h()" << std::endl;
+#endif
+
+ return true;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::finalize_solution(Ipopt::SolverReturn status,
+ Ipopt::Index n,
+ const SCALAR* x,
+ const SCALAR* z_L,
+ const SCALAR* z_U,
+ Ipopt::Index m,
+ const SCALAR* g,
+ const SCALAR* lambda,
+ SCALAR obj_value,
+ const Ipopt::IpoptData* ip_data,
+ Ipopt::IpoptCalculatedQuantities* ip_cq)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering finalize_solution() ..." << std::endl;
+#endif
+ MapConstVecXs xVec(x, n);
+ MapConstVecXs zLVec(z_L, n);
+ MapConstVecXs zUVec(z_U, n);
+ MapConstVecXs lambdaVec(lambda, m);
+
+ this->nlp_->extractIpoptSolution(xVec, zLVec, zUVec, lambdaVec);
+}
diff --git a/ct_optcon/include/ct/optcon/optcon-prespec.h b/ct_optcon/include/ct/optcon/optcon-prespec.h
new file mode 100644
index 0000000..0a1f139
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/optcon-prespec.h
@@ -0,0 +1,51 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_OPTCON_OPTCON_H_
+#define INCLUDE_CT_OPTCON_OPTCON_H_
+
+#include <ct/core/core-prespec.h>
+
+#include "costfunction/costfunction.hpp"
+#include "costfunction/costfunction-impl.hpp" //temporary solution
+
+#include "constraint/constraint.h"
+
+#include "problem/OptConProblem.h"
+#include "problem/LQOCProblem.hpp"
+#include "solver/OptConSolver.h"
+
+#include "nloc/NLOCBackendBase.hpp"
+#include "nloc/NLOCBackendST.hpp"
+#include "nloc/NLOCBackendMP.hpp"
+#include "nloc/algorithms/gnms/GNMS.hpp"
+#include "nloc/algorithms/ilqr/iLQR.hpp"
+
+#include "solver/lqp/HPIPMInterface.hpp"
+#include "solver/lqp/GNRiccatiSolver.hpp"
+#include "solver/NLOptConSolver.hpp"
+#include "solver/NLOptConSettings.hpp"
+
+#include "lqr/riccati/CARE.hpp"
+#include "lqr/riccati/DARE.hpp"
+#include "lqr/FHDTLQR.hpp"
+#include "lqr/LQR.hpp"
+
+#include "dms/dms.h"
+
+#include "mpc/MpcSettings.h"
+#include "mpc/MPC.h"
+#include "mpc/timehorizon/MpcTimeHorizon.h"
+#include "mpc/policyhandler/PolicyHandler.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler.h"
+
+/*!
+ * \warning{do not include implementation files in optcon-prespec.h}
+ */
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in optcon.h
+#endif /* INCLUDE_CT_OPTCON_OPTCON_H_ */
diff --git a/ct_optcon/include/ct/optcon/optcon.h b/ct_optcon/include/ct/optcon/optcon.h
new file mode 100644
index 0000000..224a7d3
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/optcon.h
@@ -0,0 +1,83 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_OPTCON_OPTCON_H_
+#define INCLUDE_CT_OPTCON_OPTCON_H_
+
+#include <ct/core/core.h>
+
+#include "costfunction/costfunction.hpp"
+
+#include "constraint/constraint.h"
+
+#include "problem/OptConProblem.h"
+#include "problem/LQOCProblem.hpp"
+#include "solver/OptConSolver.h"
+
+#include "nloc/NLOCBackendBase.hpp"
+#include "nloc/NLOCBackendST.hpp"
+#include "nloc/NLOCBackendMP.hpp"
+#include "nloc/algorithms/gnms/GNMS.hpp"
+#include "nloc/algorithms/ilqr/iLQR.hpp"
+
+#include "solver/lqp/HPIPMInterface.hpp"
+#include "solver/lqp/GNRiccatiSolver.hpp"
+#include "solver/NLOptConSolver.hpp"
+#include "solver/NLOptConSettings.hpp"
+
+#include "lqr/riccati/CARE.hpp"
+#include "lqr/riccati/DARE.hpp"
+#include "lqr/FHDTLQR.hpp"
+#include "lqr/LQR.hpp"
+
+#include "dms/dms.h"
+
+#include "mpc/MpcSettings.h"
+#include "mpc/MPC.h"
+#include "mpc/timehorizon/MpcTimeHorizon.h"
+#include "mpc/policyhandler/PolicyHandler.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler.h"
+
+
+// implementations
+//costfunction
+#include "costfunction/costfunction-impl.hpp"
+
+//constraints
+#include "constraint/constraint-impl.h"
+
+//problem
+#include "problem/OptConProblem-impl.h"
+#include "problem/LQOCProblem-impl.hpp"
+
+//solver
+#include "solver/lqp/GNRiccatiSolver-impl.hpp"
+#include "solver/lqp/HPIPMInterface-impl.hpp"
+#include "solver/NLOptConSolver-impl.hpp"
+
+//lqr
+#include "lqr/riccati/CARE-impl.hpp"
+#include "lqr/riccati/DARE-impl.hpp"
+#include "lqr/FHDTLQR-impl.hpp"
+#include "lqr/LQR-impl.hpp"
+
+//nloc
+#include "nloc/NLOCBackendBase-impl.hpp"
+#include "nloc/NLOCBackendST-impl.hpp"
+#include "nloc/NLOCBackendMP-impl.hpp"
+#include "nloc/algorithms/gnms/GNMS-impl.hpp"
+#include "nloc/algorithms/ilqr/iLQR-impl.hpp"
+
+//mpc
+#include "mpc/MPC-impl.h"
+#include "mpc/timehorizon/MpcTimeHorizon-impl.h"
+#include "mpc/policyhandler/PolicyHandler-impl.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h"
+
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in optcon-prespec.h
+#endif /* INCLUDE_CT_OPTCON_OPTCON_H_ */
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
new file mode 100644
index 0000000..d59428a
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -0,0 +1,245 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LQOCProblem(int N)
+ : hasBoxConstraints_(false), // by default, we assume the problem ins unconstrained
+ hasGenConstraints_(false)
+{
+ changeNumStages(N);
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isConstrained() const
+{
+ return (isBoxConstrained() | isGeneralConstrained());
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isBoxConstrained() const
+{
+ return hasBoxConstraints_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isGeneralConstrained() const
+{
+ return hasGenConstraints_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+int LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getNumberOfStages()
+{
+ return K_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumStages(int N)
+{
+ K_ = N;
+
+ A_.resize(N);
+ B_.resize(N);
+ b_.resize(N + 1);
+
+ x_.resize(N + 1);
+ u_.resize(N);
+
+ P_.resize(N);
+ q_.resize(N + 1);
+ qv_.resize(N + 1);
+ Q_.resize(N + 1);
+
+ rv_.resize(N);
+ R_.resize(N);
+
+ ux_lb_.resize(N + 1);
+ ux_ub_.resize(N + 1);
+ ux_I_.resize(N + 1);
+ nb_.resize(N + 1);
+
+ ng_.resize(N + 1);
+ d_lb_.resize(N + 1);
+ d_ub_.resize(N + 1);
+ C_.resize(N + 1);
+ D_.resize(N + 1);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
+{
+ A_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Zero());
+ B_.setConstant(core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());
+ b_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ x_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ u_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
+ P_.setConstant(core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());
+ qv_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ Q_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Zero());
+ rv_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
+ R_.setConstant(core::ControlMatrix<CONTROL_DIM, SCALAR>::Zero());
+ q_.setConstant((SCALAR)0.0);
+
+ // reset the number of box constraints
+ std::fill(nb_.begin(), nb_.end(), 0);
+
+ assert(d_lb_.size() == d_ub_.size());
+ assert(d_lb_.size() == C_.size());
+ assert(d_lb_.size() == D_.size());
+ std::fill(ng_.begin(), ng_.end(), 0);
+ for (size_t i = 0; i < d_lb_.size(); i++)
+ {
+ d_lb_[i].resize(nGenConstr, 1);
+ d_lb_[i].setZero();
+ d_ub_[i].resize(nGenConstr, 1);
+ d_ub_[i].setZero();
+ C_[i].resize(nGenConstr, STATE_DIM);
+ C_[i].setZero();
+ D_[i].resize(nGenConstr, CONTROL_DIM);
+ D_[i].setZero();
+ }
+
+ hasBoxConstraints_ = false;
+ hasGenConstraints_ = false;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateBoxConstraint(const int index,
+ const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ if ((ux_lb.rows() != ux_ub.rows()) | (ux_lb.size() != nConstr) | (sp.rows() != nConstr) |
+ (sp(sp.rows() - 1) > (STATE_DIM + CONTROL_DIM - 1)))
+ {
+ std::cout << "n.o. constraints : " << nConstr << std::endl;
+ std::cout << "ux_lb : " << ux_lb.transpose() << std::endl;
+ std::cout << "ux_ub : " << ux_ub.transpose() << std::endl;
+ std::cout << "sparsity : " << sp.transpose() << std::endl;
+ throw(std::runtime_error("LQOCProblem setIntermediateBoxConstraint: error in constraint config"));
+ }
+
+ if (index >= K_)
+ throw(std::runtime_error("LQOCProblem cannot set an intermediate Box constraint at time >= K_"));
+
+ nb_[index] = nConstr;
+ ux_lb_[index].template topRows(nConstr) = ux_lb;
+ ux_ub_[index].template topRows(nConstr) = ux_ub;
+ ux_I_[index].template topRows(nConstr) = sp;
+
+ hasBoxConstraints_ = true;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ for (int i = 0; i < K_; i++)
+ setIntermediateBoxConstraint(i, nConstr, ux_lb, ux_ub, sp);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTerminalBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ if (nConstr > 0)
+ {
+ if ((ux_lb.rows() != ux_ub.rows()) | (ux_lb.size() != nConstr) | (sp.rows() != nConstr) |
+ (sp(sp.rows() - 1) > (STATE_DIM - 1)))
+ {
+ std::cout << "n.o. constraints : " << nConstr << std::endl;
+ std::cout << "ux_lb : " << ux_lb.transpose() << std::endl;
+ std::cout << "ux_ub : " << ux_ub.transpose() << std::endl;
+ std::cout << "sparsity : " << sp.transpose() << std::endl;
+ throw(std::runtime_error("LQOCProblem setTerminalBoxConstraint: error in constraint config"));
+ }
+
+ nb_[K_] = nConstr;
+ ux_lb_[K_].template topRows(nConstr) = ux_lb;
+ ux_ub_[K_].template topRows(nConstr) = ux_ub;
+ ux_I_[K_].template topRows(nConstr) = sp;
+
+ hasBoxConstraints_ = true;
+ }
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const constr_vec_t& d_lb,
+ const constr_vec_t& d_ub,
+ const constr_state_jac_t& C,
+ const constr_control_jac_t& D)
+{
+ d_lb_.setConstant(d_lb);
+ d_ub_.setConstant(d_ub);
+ C_.setConstant(C);
+ D_.setConstant(D);
+
+ hasGenConstraints_ = true;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuadraticProblem(
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+ ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
+ ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
+ ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
+ double dt)
+{
+ setZero();
+
+ core::StateMatrix<STATE_DIM, SCALAR> A;
+ core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
+ linearSystem.getAandB(x0, u0, 0, A, B);
+
+ A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
+ B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
+ b_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, stateOffset);
+
+
+ // feed current state and control to cost function
+ costFunction.setCurrentStateAndControl(x0, u0, 0);
+
+ // derivative of cost with respect to state
+ qv_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, costFunction.stateDerivativeIntermediate() * dt);
+ Q_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_ + 1, costFunction.stateSecondDerivativeIntermediate() * dt);
+
+ // derivative of cost with respect to control and state
+ P_ =
+ core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, costFunction.stateControlDerivativeIntermediate() * dt);
+
+ // derivative of cost with respect to control
+ rv_ = core::ControlVectorArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlDerivativeIntermediate() * dt);
+
+ R_ = core::ControlMatrixArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlSecondDerivativeIntermediate() * dt);
+
+ Q_[K_] = costFunction.stateSecondDerivativeTerminal();
+ qv_[K_] = costFunction.stateDerivativeTerminal();
+
+ x_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, x0);
+ u_ = core::ControlVectorArray<CONTROL_DIM, SCALAR>(K_, u0);
+
+ hasBoxConstraints_ = false;
+ hasGenConstraints_ = false;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
new file mode 100644
index 0000000..2eb5572
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
@@ -0,0 +1,230 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \brief Defines a Linear-Quadratic Optimal Control Problem, which is optionally constrained.
+ *
+ * This class defines a Linear Quadratic Optimal Control (LQOC) Problem, consisting of
+ * - affine system dynamics
+ * - reference trajectories (arrays!) for state and control
+ * - LQ approximation of the cost function
+ *
+ * The unconstrained LQ problem hence has the following form:
+ * \f[
+ * \min_{\delta \mathbf{u}_n, \delta \mathbf{x}_n}
+ * \bigg \{
+ * q_N +\delta \mathbf{x}_N^\top \mathbf{q}_N +\frac{1}{2}\delta \mathbf{x}_N^\top \mathbf{Q}_N\delta \mathbf{x}_N
+ * +\sum_{n=0}^{N-1} q_n + \delta \mathbf{x}_n^\top \mathbf{q}_n
+ * + \delta \mathbf{u}_n^\top \mathbf{r}_n
+ * + \frac{1}{2}\delta \mathbf{x}_n^\top\mathbf{Q}_n\delta \mathbf{x}_n
+ * +\frac{1}{2}\delta \mathbf{u}_n^\top \mathbf{R}_n\delta \mathbf{u}_n
+ * + \delta \mathbf{u}_n^\top \mathbf{P}_n\delta \mathbf{x}_n
+ * \bigg \}
+ * \f]
+ * subject to
+ * \f[
+ * \delta \mathbf x_{n+1} = \mathbf A_n \delta \mathbf x_n + \mathbf B_n \delta \mathbf u_n +\mathbf b_n
+ * \f]
+ * with
+ * \f$ \delta \mathbf x_n = \mathbf x_n - \hat \mathbf x_n \f$ and \f$ \delta \mathbf u_n = \mathbf u_n - \hat \mathbf u_n \f$
+ *
+ * The reference trajectories for state and control are here denoted as \f$ \hat \mathbf x_i, \
+ * \hat \mathbf u_i \quad \forall i = 0, 1, \ldots \f$
+ *
+ * The constrained LQ problem additionally implements the box constraints
+ * \f$ \mathbf x_{lb} \leq \mathbf x_n \leq \mathbf x_{ub} \ \forall i=1,2,\ldots,N \f$
+ * and
+ * \f$ \mathbf u_{lb} \leq \mathbf u_n \leq \mathbf u_{ub} \ \forall i=0,1,\ldots,N-1\f$
+ * and the general inequality constraints
+ * \f[
+ * \mathbf d_{lb} \leq \mathbf C_n \mathbf x_n + \mathbf D_n \mathbf u_n \leq \mathbf d_{ub} \ \forall i=0,1,\ldots,N
+ * \f]
+ * which are both always kept in absolute coordinates.
+ *
+ * \note The box constraint containers within this class are made fixed-size. Solvers can get the
+ * actual number of box constraints from a a dedicated container nb_
+ *
+ * \todo refactor all to be in global coordinates
+ * \todo Refactor the initializing methods such that const-references can be handed over.
+ */
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
+class LQOCProblem
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using constr_vec_t = Eigen::Matrix<SCALAR, -1, -1>;
+ using constr_state_jac_t = Eigen::Matrix<SCALAR, -1, -1>;
+ using constr_control_jac_t = Eigen::Matrix<SCALAR, -1, -1>;
+
+ using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;
+ using constr_state_jac_array_t = ct::core::DiscreteArray<constr_state_jac_t>;
+ using constr_control_jac_array_t = ct::core::DiscreteArray<constr_control_jac_t>;
+
+ using box_constr_t = Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM, 1>;
+ using box_constr_array_t = ct::core::DiscreteArray<box_constr_t>;
+
+ //! a vector indicating which box constraints are active and which not
+ using box_constr_sparsity_t = Eigen::Matrix<int, STATE_DIM + CONTROL_DIM, 1>;
+ using box_constr_sparsity_array_t = ct::core::DiscreteArray<box_constr_sparsity_t>;
+
+ using VectorXi = Eigen::VectorXi;
+
+ //! constructor
+ LQOCProblem(int N = 0);
+
+ //! returns the number of discrete time steps in the LOCP, including terminal stage
+ int getNumberOfStages();
+
+ //! change the number of discrete time steps in the LOCP
+ void changeNumStages(int N);
+
+ /*!
+ * @brief set all member variables to zero
+ * @param nGenConstr by default, we resize the general constraint containers to zero
+ */
+ void setZero(const int& nGenConstr = 0);
+
+ /*!
+ * \brief set intermediate box constraints at a specific index
+ * @param index the index
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setIntermediateBoxConstraint(const int index,
+ const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set uniform box constraints, with the same constraint being applied at each intermediate stage
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setIntermediateBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set box constraints for terminal stage
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setTerminalBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set general (in)equaltiy constraints, with the same constraint applied at each stage
+ * @param d_lb general constraint lower bound
+ * @param d_ub general constraint upper bound
+ * @param C general constraint state jacobian
+ * @param D general constraint control jacobian
+ */
+ void setGeneralConstraints(const constr_vec_t& d_lb,
+ const constr_vec_t& d_ub,
+ const constr_state_jac_t& C,
+ const constr_control_jac_t& D);
+
+ /*!
+ * \brief a convenience method which constructs an unconstrained LQOC Problem from an LTI system and continuous-time quadratic cost
+ * The discretization of the cost functions happens within this function. It employs an Euler-Discretization
+ * @param x0 the initial state
+ * @param u0 the current (and desired control)
+ * @param linearSystem the discrete-time LTI system
+ * @param costFunction the continuous-time cost function
+ * @param stateOffset the offset for the affine system dynamics demanded by the LQOC Solver
+ * @param dt the sampling time, required for discretization
+ */
+ void setFromTimeInvariantLinearQuadraticProblem(ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+ ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
+ ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
+ ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
+ double dt);
+
+ //! return a flag indicating whether this LQOC Problem is constrained or not
+ bool isConstrained() const;
+
+ bool isBoxConstrained() const;
+ bool isGeneralConstrained() const;
+
+ //! affine, time-varying system dynamics in discrete time
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> A_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> B_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> b_;
+
+ //! reference state trajectory
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_;
+
+ //! reference control trajectory
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_;
+
+ //! constant term of in the LQ approximation of the cost function
+ ct::core::ScalarArray<SCALAR> q_;
+
+ //! LQ approximation of the pure state penalty, including terminal state penalty
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> qv_;
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> Q_;
+
+ //! LQ approximation of the pure control penalty
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> rv_;
+ ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> R_;
+
+ //! LQ approximation of the cross terms of the cost function
+ ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> P_;
+
+ //! lower bound of box constraints in order [u_lb; x_lb]. Stacked for memory efficiency.
+ box_constr_array_t ux_lb_;
+ //! upper bound of box constraints in order [u_ub; x_ub]. Stacked for memory efficiency.
+ box_constr_array_t ux_ub_;
+ /*!
+ * \brief container for the box constraint sparsity pattern
+ * An example for how an element of this array might look like: [0 1 4 7]
+ * This would mean that box constraints act on elements 0, 1, 4 and 7 of the
+ * combined vector of decision variables [u; x]
+ */
+ box_constr_sparsity_array_t ux_I_;
+ //! the number of box constraints at every stage.
+ std::vector<int> nb_;
+
+ //! general constraint lower bound
+ constr_vec_array_t d_lb_;
+ //! general constraint upper bound
+ constr_vec_array_t d_ub_;
+ //! linear general constraint matrices
+ constr_state_jac_array_t C_;
+ constr_control_jac_array_t D_;
+ //! number of general inequality constraints
+ std::vector<int> ng_;
+
+ //! bool indicating if the optimization problem is box-constrained
+ bool hasBoxConstraints_;
+
+ //! bool indicating if the optimization problem hs general inequality constraints
+ bool hasGenConstraints_;
+
+private:
+ //! the number of discrete time steps in the LOCP, including terminal stage
+ int K_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h b/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h
new file mode 100644
index 0000000..509f95c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h
@@ -0,0 +1,202 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem)
+ : tf_(0.0),
+ x0_(state_vector_t::Zero()),
+ controlledSystem_(nonlinDynamics),
+ costFunction_(costFunction),
+ linearizedSystem_(linearSystem),
+ boxConstraints_(nullptr),
+ generalConstraints_(nullptr)
+{
+ if (linearSystem == nullptr) // no linearization provided
+ {
+ linearizedSystem_ = std::shared_ptr<core::SystemLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new core::SystemLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>(controlledSystem_));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics, costFunction, linearSystem) // delegating constructor
+{
+ tf_ = tf;
+ x0_ = x0;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics, costFunction, linearSystem) // delegating constructor
+{
+ boxConstraints_ = boxConstraints;
+ generalConstraints_ = generalConstraints;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics,
+ costFunction,
+ boxConstraints,
+ generalConstraints,
+ linearSystem) // delegating constructor
+{
+ tf_ = tf;
+ x0_ = x0;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::verify() const
+{
+ if (!controlledSystem_)
+ {
+ throw std::runtime_error("Dynamic system not set");
+ }
+ if (!linearizedSystem_)
+ {
+ throw std::runtime_error("Linearized system not set");
+ }
+ if (!costFunction_)
+ {
+ throw std::runtime_error("Cost function not set");
+ }
+ if (tf_ < 0.0)
+ {
+ throw std::runtime_error("Time horizon should not be negative");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::DynamicsPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getNonlinearSystem() const
+{
+ return controlledSystem_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LinearPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getLinearSystem() const
+{
+ return linearizedSystem_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getCostFunction() const
+{
+ return costFunction_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setNonlinearSystem(const DynamicsPtr_t dyn)
+{
+ controlledSystem_ = dyn;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setLinearSystem(const LinearPtr_t lin)
+{
+ linearizedSystem_ = lin;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setCostFunction(const CostFunctionPtr_t cost)
+{
+ costFunction_ = cost;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setBoxConstraints(const ConstraintPtr_t constraint)
+{
+ boxConstraints_ = constraint;
+ if (!boxConstraints_->isInitialized())
+ boxConstraints_->initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const ConstraintPtr_t constraint)
+{
+ generalConstraints_ = constraint;
+ if (!generalConstraints_->isInitialized())
+ generalConstraints_->initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getBoxConstraints() const
+{
+ return boxConstraints_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getGeneralConstraints() const
+{
+ return generalConstraints_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getInitialState() const
+{
+ return x0_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInitialState(const state_vector_t& x0)
+{
+ x0_ = x0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const SCALAR& OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getTimeHorizon() const
+{
+ return tf_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTimeHorizon(const SCALAR& tf)
+{
+ tf_ = tf;
+}
+
+
+} // optcon
+} // ct
diff --git a/ct_optcon/include/ct/optcon/problem/OptConProblem.h b/ct_optcon/include/ct/optcon/problem/OptConProblem.h
new file mode 100644
index 0000000..96d6b0b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/OptConProblem.h
@@ -0,0 +1,225 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/constraint/LinearConstraintContainer.h>
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \defgroup OptConProblem OptConProblem
+ *
+ * \brief Class that defines how to set up an Optimal Control Problem
+ *
+ * An finite-horizon optimal control problem is generally defined through
+ * - nonlinear system dynamics
+ * - cost function (intermediate + terminal cost)
+ * - initial state
+ * - box constraints
+ * - general constraints
+ * - an overall time horizon
+ *
+ * Note that in most cases, the user can also provide a pointer to the linearized system dynamics. This is optional, and
+ * in case it is not provided, numerical differentiation will be applied to approximate the linearized dynamics.
+ *
+ * \warning Using numerical differentiation is inefficient and typically slow.
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class OptConProblem
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ // typedefs
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> DynamicsPtr_t;
+ typedef std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> LinearPtr_t;
+ typedef std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> CostFunctionPtr_t;
+ typedef std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> ConstraintPtr_t;
+
+ OptConProblem();
+
+ /*!
+ * @brief Construct a simple unconstrained Optimal Control Problem
+ * \warning time and initial state to be specified later
+ *
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives. If the
+ * user does not specify the derivatives, they are generated automatically using numerical differentiation. Warning: this is slow
+ */
+ OptConProblem(DynamicsPtr_t nonlinDynamics, CostFunctionPtr_t costFunction, LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a simple unconstrained optimal control problem, with initial state and final time as constructor arguments
+ * @param tf The optimal control problem final time horizon
+ * @param x0 The initial system state
+ * @param nonlinDynamics The nonlinear system dynamics
+ * @param costFunction A quadratic cost function
+ * @param linearSystem (optional) Linearized System Dynamics.
+ */
+ OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a constrained Optimal Control Problem
+ *
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param boxConstraints the box constraints
+ * @param generalConstraints the general constraints
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives.
+ *
+ * \warning time and initial state to be specified later
+ * \warning If the user does not specify the derivatives, they are generated automatically using numerical differentiation. This is slow
+ */
+ OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a constrained Optimal Control Problem
+ *
+ * @param tf The optimal control problem final time horizon
+ * @param x0 The initial system state
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param boxConstraints the box constraints
+ * @param generalConstraints the general constraints
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives.
+ *
+ * \warning time and initial state to be specified later
+ * \warning If the user does not specify the derivatives, they are generated automatically using numerical differentiation. This is slow
+ */
+ OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem = nullptr);
+
+ //! check if all the ingredients for an unconstrained optimal control problem are there
+ void verify() const;
+
+ /*!
+ * returns a pointer to the controlled system
+ * */
+ const DynamicsPtr_t getNonlinearSystem() const;
+
+ /*!
+ * returns a pointer to the linear system approximation
+ * */
+ const LinearPtr_t getLinearSystem() const;
+
+ /*!
+ * returns a pinter to the cost function
+ * */
+ const CostFunctionPtr_t getCostFunction() const;
+
+ /*!
+ * returns a pointer to the controlled system
+ * */
+ void setNonlinearSystem(const DynamicsPtr_t dyn);
+
+ /*!
+ * returns a pointer to the linear system approximation
+ * */
+ void setLinearSystem(const LinearPtr_t lin);
+
+ /*!
+ * returns a pinter to the cost function
+ * */
+ void setCostFunction(const CostFunctionPtr_t cost);
+
+ /*!
+ * set box constraints
+ * @param constraint pointer to box constraint
+ */
+ void setBoxConstraints(const ConstraintPtr_t constraint);
+
+ /*!
+ * set general constraints
+ * @param constraint pointer to a general constraint
+ */
+ void setGeneralConstraints(const ConstraintPtr_t constraint);
+
+ /**
+ * @brief Retrieve the box constraints
+ *
+ * @return The box constraints.
+ */
+ const ConstraintPtr_t getBoxConstraints() const;
+
+ /**
+ * @brief Retrieves the general constraints
+ *
+ * @return The the general constraints
+ */
+ const ConstraintPtr_t getGeneralConstraints() const;
+
+ /*!
+ * get initial state (called by solvers)
+ * */
+ const state_vector_t getInitialState() const;
+
+ /*!
+ * set initial state for first subsystem
+ * */
+ void setInitialState(const state_vector_t& x0);
+
+ /*!
+ * get the current time horizon
+ * @return Time Horizon
+ */
+ const SCALAR& getTimeHorizon() const;
+
+ /*!
+ * Update the current time horizon in the Opt.Control Problem (required for example for replanning)
+ * @param tf new time horizon
+ */
+ void setTimeHorizon(const SCALAR& tf);
+
+
+private:
+ SCALAR tf_; //! end time
+
+ state_vector_t x0_; //! initial state
+
+ DynamicsPtr_t controlledSystem_; //! the nonlinear system
+ CostFunctionPtr_t costFunction_; //! a quadratic cost function
+ LinearPtr_t linearizedSystem_; //! the linear approximation of the nonlinear system
+
+ /*!
+ * @brief container of all the state and input box constraints of the problem
+ * Expected form:
+ * \f$ u_{lb} \leq u \leq u_{ub} \f$ and \f$ x_{lb} \leq x \leq x_{ub} \f$
+ */
+ ConstraintPtr_t boxConstraints_;
+
+ /*!
+ * @brief container of all the general constraints of the problem
+ * Expected form:
+ * \f$ d_{lb} \leq g(x,u) \leq d_{ub} \f$
+ */
+ ConstraintPtr_t generalConstraints_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp
new file mode 100644
index 0000000..5eb7c05
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp
@@ -0,0 +1,612 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <map>
+
+#include <boost/property_tree/info_parser.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+//! GNMS Line Search Settings
+/*!
+ * \ingroup GNMS
+ *
+ * The Line Search Settings are part of the general settings struct and hold parameters to customize the line-search for the NLOptCon controller update.
+ */
+struct LineSearchSettings
+{
+ //! default constructor for the NLOptCon line-search settings
+ LineSearchSettings()
+ : active(true),
+ adaptive(false),
+ maxIterations(10),
+ alpha_0(1.0),
+ alpha_max(1.0),
+ n_alpha(0.5),
+ debugPrint(false)
+ {
+ }
+
+ //! check if the currently set line-search parameters are meaningful
+ bool parametersOk() const { return (alpha_0 > 0.0) && (n_alpha > 0.0) && (n_alpha < 1.0) && (alpha_max > 0.0); }
+ bool active; /*!< Flag whether or not to perform line search */
+ bool adaptive; /*!< Flag whether alpha_0 gets updated based on previous iteration */
+ size_t maxIterations; /*!< Maximum number of iterations during line search */
+ double alpha_0; /*!< Initial step size for line search. Use 1 for step size as suggested by NLOptCon */
+ double alpha_max; /*!< Maximum step size for line search. This is the limit when adapting alpha_0. */
+ double
+ n_alpha; /*!< Factor by which the line search step size alpha gets multiplied with after each iteration. Usually 0.5 is a good value. */
+ bool debugPrint; /*!< Print out debug information during line-search*/
+
+
+ //! print the current line search settings to console
+ void print() const
+ {
+ std::cout << "Line Search Settings: " << std::endl;
+ std::cout << "=====================" << std::endl;
+ std::cout << "active:\t" << active << std::endl;
+ std::cout << "adaptive:\t" << adaptive << std::endl;
+ std::cout << "maxIter:\t" << maxIterations << std::endl;
+ std::cout << "alpha_0:\t" << alpha_0 << std::endl;
+ std::cout << "alpha_max:\t" << alpha_max << std::endl;
+ std::cout << "n_alpha:\t" << n_alpha << std::endl;
+ std::cout << "debugPrint:\t" << debugPrint << std::endl;
+ std::cout << " =======" << std::endl;
+ std::cout << std::endl;
+ }
+
+ //! load line search settings from file
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "line_search")
+ {
+ if (verbose)
+ std::cout << "Trying to load line search settings from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ active = pt.get<bool>(ns + ".active");
+ maxIterations = pt.get<size_t>(ns + ".maxIterations");
+ alpha_0 = pt.get<double>(ns + ".alpha_0");
+ n_alpha = pt.get<double>(ns + ".n_alpha");
+ debugPrint = pt.get<bool>(ns + ".debugPrint");
+ alpha_max = alpha_0;
+ adaptive = false;
+
+ try
+ {
+ adaptive = pt.get<bool>(ns + ".adaptive");
+ alpha_max = pt.get<double>(ns + ".alpha_max");
+ } catch (...)
+ {
+ }
+
+ if (verbose)
+ {
+ std::cout << "Loaded line search settings from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+};
+
+
+//! LQOC Solver settings
+/*!
+ * Settings for solving each linear-quadratic (constrained) sub-problem
+ */
+struct LQOCSolverSettings
+{
+public:
+ LQOCSolverSettings() : num_lqoc_iterations(5), lqoc_debug_print(false) {}
+ int num_lqoc_iterations; //! number of allowed sub-iterations of LQOC solver per NLOC main iteration
+ bool lqoc_debug_print;
+
+ void print() const
+ {
+ std::cout << "======================= LQOCSolverSettings =====================" << std::endl;
+ std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl;
+ std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl;
+ }
+
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings")
+ {
+ if (verbose)
+ std::cout << "Trying to load LQOCSolverSettings config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ num_lqoc_iterations = pt.get<int>(ns + ".num_lqoc_iterations");
+ } catch (...)
+ {
+ }
+ try
+ {
+ lqoc_debug_print = pt.get<bool>(ns + ".lqoc_debug_print");
+ } catch (...)
+ {
+ }
+ }
+};
+
+
+/*!
+ * \ingroup NLOptCon
+ *
+ * \brief Settings for the NLOptCon algorithm.
+ */
+class NLOptConSettings
+{
+public:
+ typedef typename core::SensitivityApproximationSettings::APPROXIMATION APPROXIMATION;
+
+ //! the nonlinear optimal control problem solving algorithm
+ enum NLOCP_ALGORITHM
+ {
+ GNMS = 0,
+ ILQR,
+ NUM_TYPES
+ };
+
+ //! the linear optimal control problem solver in the background
+ enum LQOCP_SOLVER
+ {
+ GNRICCATI_SOLVER = 0,
+ HPIPM_SOLVER = 1
+ };
+
+
+ //! NLOptCon Settings default constructor
+ /*!
+ * sets all settings to default values.
+ */
+ NLOptConSettings()
+ : integrator(ct::core::IntegrationType::RK4),
+ discretization(APPROXIMATION::BACKWARD_EULER),
+ timeVaryingDiscretization(false),
+ nlocp_algorithm(GNMS),
+ lqocp_solver(GNRICCATI_SOLVER),
+ loggingPrefix("alg"),
+ closedLoopShooting(false), //! by default, we do open-loop shooting
+ epsilon(1e-5),
+ dt(0.001),
+ K_sim(1), //! by default, there is only one sub-integration step
+ K_shot(1), //! by default the shot length is equal to the control length
+ min_cost_improvement(1e-5), //! cost needs to be at least 1e-5 better before we assume convergence
+ maxDefectSum(1e-5),
+ meritFunctionRho(0.0),
+ meritFunctionRhoConstraints(1.0),
+ max_iterations(100),
+ fixedHessianCorrection(false),
+ recordSmallestEigenvalue(false),
+ nThreads(4),
+ nThreadsEigen(4),
+ lineSearchSettings(),
+ debugPrint(false),
+ printSummary(true),
+ useSensitivityIntegrator(false)
+ {
+ }
+
+ ct::core::IntegrationType integrator; //! which integrator to use during the NLOptCon forward rollout
+ APPROXIMATION discretization;
+ bool timeVaryingDiscretization;
+ NLOCP_ALGORITHM nlocp_algorithm; //! which nonlinear optimal control algorithm is to be used
+ LQOCP_SOLVER lqocp_solver; //! the solver for the linear-quadratic optimal control problem
+ std::string loggingPrefix; //! the prefix to be stored before the matfile name for logging
+ bool closedLoopShooting; //! use feedback gains during forward integration (true) or not (false)
+ double epsilon; //! Eigenvalue correction factor for Hessian regularization
+ double dt; //! sampling time for the control input (seconds)
+ int K_sim; //! number of sub-integration-steps
+ int K_shot; //! duration of a shot as an integer multiple of dt
+ double min_cost_improvement; //! minimum cost improvement between two interations to assume convergence
+ double maxDefectSum; //! maximum sum of squared defects (assume covergence if lower than this number)
+ double meritFunctionRho; //! trade off between internal (defect)constraint violation and cost
+ double meritFunctionRhoConstraints; //! trade off between external (general and path) constraint violation and cost
+ int max_iterations; //! the maximum admissible number of NLOptCon main iterations \warning make sure to select this number high enough allow for convergence
+ bool fixedHessianCorrection; //! perform Hessian regularization by incrementing the eigenvalues by epsilon.
+ bool recordSmallestEigenvalue; //! save the smallest eigenvalue of the Hessian
+ int nThreads; //! number of threads, for MP version
+ size_t
+ nThreadsEigen; //! number of threads for eigen parallelization (applies both to MP and ST) Note. in order to activate Eigen parallelization, compile with '-fopenmp'
+ LineSearchSettings lineSearchSettings; //! the line search settings
+ LQOCSolverSettings lqoc_solver_settings;
+ bool debugPrint;
+ bool printSummary;
+ bool useSensitivityIntegrator;
+
+
+ //! compute the number of discrete time steps for an arbitrary input time interval
+ /*!
+ * @param timeHorizon the time horizon of interest, e.g. overall optimal control time horizon or shot-length
+ * @return the resulting number of steps, minimum 1 steps long
+ *
+ * \todo naming it K is confusing, should better be N.
+ */
+ int computeK(double timeHorizon) const
+ {
+ if (timeHorizon < 0.0)
+ {
+ throw std::runtime_error("time Horizon is negative");
+ }
+ return std::max(1, (int)std::lround(timeHorizon / dt));
+ }
+
+ double getSimulationTimestep() const { return (dt / (double)K_sim); }
+ //! print the current NLOptCon settings to console
+ void print() const
+ {
+ std::cout << "======================= NLOptCon Settings =====================" << std::endl;
+ std::cout << "===============================================================" << std::endl;
+ std::cout << "integrator: " << integratorToString.at(integrator) << std::endl;
+ std::cout << "discretization: " << discretizationToString.at(discretization) << std::endl;
+ std::cout << "time varying discretization: " << timeVaryingDiscretization << std::endl;
+ std::cout << "nonlinear OCP algorithm: " << nlocp_algorithmToString.at(nlocp_algorithm) << std::endl;
+ std::cout << "linear-quadratic OCP solver: " << locp_solverToString.at(lqocp_solver) << std::endl;
+ std::cout << "closed loop shooting:\t" << closedLoopShooting << std::endl;
+ std::cout << "dt:\t" << dt << std::endl;
+ std::cout << "K_sim:\t" << K_sim << std::endl;
+ std::cout << "K_shot:\t" << K_shot << std::endl;
+ std::cout << "maxIter:\t" << max_iterations << std::endl;
+ std::cout << "min cost improvement:\t" << min_cost_improvement << std::endl;
+ std::cout << "max defect sum:\t" << maxDefectSum << std::endl;
+ std::cout << "merit function rho defects:\t" << meritFunctionRho << std::endl;
+ std::cout << "merit function rho constraints:\t" << meritFunctionRhoConstraints << std::endl;
+ std::cout << "fixedHessianCorrection:\t" << fixedHessianCorrection << std::endl;
+ std::cout << "recordSmallestEigenvalue:\t" << recordSmallestEigenvalue << std::endl;
+ std::cout << "epsilon:\t" << epsilon << std::endl;
+ std::cout << "nThreads:\t" << nThreads << std::endl;
+ std::cout << "nThreadsEigen:\t" << nThreadsEigen << std::endl;
+ std::cout << "loggingPrefix:\t" << loggingPrefix << std::endl;
+ std::cout << "debugPrint:\t" << debugPrint << std::endl;
+ std::cout << "printSummary:\t" << printSummary << std::endl;
+ std::cout << "useSensitivityIntegrator:\t" << useSensitivityIntegrator << std::endl;
+ std::cout << std::endl;
+
+ lineSearchSettings.print();
+
+ lqoc_solver_settings.print();
+
+ std::cout << "===============================================================" << std::endl;
+ }
+
+ //! perform a quick check if the given NLOptCon settings fulfil minimum requirements
+ /*!
+ * \warning This check cannot guarantee that the control problem is well parameterized
+ */
+ bool parametersOk() const
+ {
+ if (dt <= 0)
+ {
+ std::cout << "Invalid parameter dt in NLOptConSettings, needs to be > 0. dt currently is " << dt
+ << std::endl;
+ return false;
+ }
+
+ if (K_sim <= 0)
+ {
+ std::cout << "Invalid parameter K_sim in NLOptConSettings, needs to be >= 1. K_sim currently is " << K_sim
+ << std::endl;
+ return false;
+ }
+
+ if (K_shot <= 0)
+ {
+ std::cout << "Invalid parameter K_shot in NLOptConSettings, needs to be >= 1. K_shot currently is "
+ << K_shot << std::endl;
+ return false;
+ }
+
+ if ((K_shot > 1) && (nlocp_algorithm == ILQR))
+ {
+ std::cout << "Invalid parameter: for iLQR K_shot needs to be 1. K_shot currently is " << K_shot
+ << std::endl;
+ return false;
+ }
+
+ if (nThreads > 100 || nThreadsEigen > 100)
+ {
+ std::cout << "Number of threads should not exceed 100." << std::endl;
+ return false;
+ }
+ return (lineSearchSettings.parametersOk());
+ }
+
+
+ //! load NLOptCon Settings from file
+ /*!
+ *
+ * @param filename path to the settings file
+ * @param verbose print out settings after reading them
+ * @param ns (optional) namspace of parameter file
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "alg")
+ {
+ if (verbose)
+ std::cout << "Trying to load NLOptCon config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ epsilon = pt.get<double>(ns + ".epsilon");
+ } catch (...)
+ {
+ }
+ try
+ {
+ timeVaryingDiscretization = pt.get<bool>(ns + ".timeVaryingDiscretization");
+ } catch (...)
+ {
+ }
+ try
+ {
+ min_cost_improvement = pt.get<double>(ns + ".min_cost_improvement");
+ } catch (...)
+ {
+ }
+ try
+ {
+ maxDefectSum = pt.get<double>(ns + ".maxDefectSum");
+ } catch (...)
+ {
+ }
+ try
+ {
+ meritFunctionRho = pt.get<double>(ns + ".meritFunctionRho");
+ } catch (...)
+ {
+ }
+ try
+ {
+ meritFunctionRhoConstraints = pt.get<double>(ns + ".meritFunctionRhoConstraints");
+ } catch (...)
+ {
+ }
+ try
+ {
+ max_iterations = pt.get<int>(ns + ".max_iterations");
+ } catch (...)
+ {
+ }
+ try
+ {
+ nThreads = pt.get<int>(ns + ".nThreads");
+ } catch (...)
+ {
+ }
+ try
+ {
+ loggingPrefix = pt.get<std::string>(ns + ".loggingPrefix");
+ } catch (...)
+ {
+ }
+ try
+ {
+ closedLoopShooting = pt.get<bool>(ns + ".closedLoopShooting");
+ } catch (...)
+ {
+ }
+ try
+ {
+ debugPrint = pt.get<bool>(ns + ".debugPrint");
+ } catch (...)
+ {
+ }
+ try
+ {
+ printSummary = pt.get<bool>(ns + ".printSummary");
+ } catch (...)
+ {
+ }
+ try
+ {
+ useSensitivityIntegrator = pt.get<bool>(ns + ".useSensitivityIntegrator");
+ } catch (...)
+ {
+ }
+ try
+ {
+ dt = pt.get<double>(ns + ".dt");
+ } catch (...)
+ {
+ }
+ try
+ {
+ K_sim = pt.get<int>(ns + ".K_sim");
+ } catch (...)
+ {
+ }
+ try
+ {
+ K_shot = pt.get<int>(ns + ".K_shot");
+ } catch (...)
+ {
+ }
+ try
+ {
+ nThreadsEigen = pt.get<size_t>(ns + ".nThreadsEigen");
+ } catch (...)
+ {
+ }
+ try
+ {
+ recordSmallestEigenvalue = pt.get<bool>(ns + ".recordSmallestEigenvalue");
+ } catch (...)
+ {
+ }
+ try
+ {
+ fixedHessianCorrection = pt.get<bool>(ns + ".fixedHessianCorrection");
+ } catch (...)
+ {
+ }
+
+ try
+ {
+ lineSearchSettings.load(filename, verbose, ns + ".line_search");
+ } catch (...)
+ {
+ }
+ try
+ {
+ lqoc_solver_settings.load(filename, verbose, ns + ".lqoc_solver_settings");
+ } catch (...)
+ {
+ }
+
+
+ try
+ {
+ std::string integratorStr = pt.get<std::string>(ns + ".integrator");
+ if (stringToIntegrator.find(integratorStr) != stringToIntegrator.end())
+ {
+ integrator = stringToIntegrator[integratorStr];
+ }
+ else
+ {
+ std::cout << "Invalid integrator specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringToIntegrator.begin(); it != stringToIntegrator.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ std::string discretizationStr = pt.get<std::string>(ns + ".discretization");
+ if (stringToDiscretization.find(discretizationStr) != stringToDiscretization.end())
+ {
+ discretization = stringToDiscretization[discretizationStr];
+ }
+ else
+ {
+ std::cout << "Invalid discretization specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringToDiscretization.begin(); it != stringToDiscretization.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ std::string nlocp_algorithmStr = pt.get<std::string>(ns + ".nlocp_algorithm");
+ if (stringTonlocp_algorithm.find(nlocp_algorithmStr) != stringTonlocp_algorithm.end())
+ {
+ nlocp_algorithm = stringTonlocp_algorithm[nlocp_algorithmStr];
+ }
+ else
+ {
+ std::cout << "Invalid nlocp_algorithm specified in config, should be one of the following:"
+ << std::endl;
+
+ for (auto it = stringTonlocp_algorithm.begin(); it != stringTonlocp_algorithm.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+
+ std::string locp_solverStr = pt.get<std::string>(ns + ".locp_solver");
+ if (stringTolocp_solver.find(locp_solverStr) != stringTolocp_solver.end())
+ {
+ lqocp_solver = stringTolocp_solver[locp_solverStr];
+ }
+ else
+ {
+ std::cout << "Invalid locp_solver specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringTolocp_solver.begin(); it != stringTolocp_solver.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ } catch (...)
+ {
+ }
+
+ if (verbose)
+ {
+ std::cout << "Loaded NLOptCon config from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+
+ //! load settings from config file and return as settings struct
+ /*!
+ * @param filename the path to the settings file
+ * @param verbose print settings
+ * @param ns (optional) settings namespace
+ * @return the newly generated settings struct
+ */
+ static NLOptConSettings fromConfigFile(const std::string& filename,
+ bool verbose = true,
+ const std::string& ns = "alg")
+ {
+ NLOptConSettings settings;
+ settings.load(filename, true, ns);
+ return settings;
+ }
+
+private:
+ //! mappings for integrator types
+ std::map<ct::core::IntegrationType, std::string> integratorToString = {{ct::core::IntegrationType::EULER, "Euler"},
+ {ct::core::IntegrationType::RK4, "Runge-Kutta 4th Order"},
+ {ct::core::IntegrationType::MODIFIED_MIDPOINT, "Modified midpoint"},
+ {ct::core::IntegrationType::ODE45, "ode45"}, {ct::core::IntegrationType::RK5VARIABLE, "RK5 variable step"},
+ {ct::core::IntegrationType::RK78, "RK78"}, {ct::core::IntegrationType::BULIRSCHSTOER, "Bulirsch-Stoer"},
+ {ct::core::IntegrationType::EULERCT, "Euler (CT)"},
+ {ct::core::IntegrationType::RK4CT, "Runge-Kutta 4th Order (CT"},
+ {ct::core::IntegrationType::EULER_SYM, "Symplectic Euler"},
+ {ct::core::IntegrationType::RK_SYM, "Symplectic Runge Kutta"}};
+
+ std::map<std::string, ct::core::IntegrationType> stringToIntegrator = {{"Euler", ct::core::IntegrationType::EULER},
+ {"RK4", ct::core::IntegrationType::RK4}, {"MODIFIED_MIDPOINT", ct::core::IntegrationType::MODIFIED_MIDPOINT},
+ {"ODE45", ct::core::IntegrationType::ODE45}, {"RK5VARIABLE", ct::core::IntegrationType::RK5VARIABLE},
+ {"RK78", ct::core::IntegrationType::RK78}, {"BULIRSCHSTOER", ct::core::IntegrationType::BULIRSCHSTOER},
+ {"EulerCT", ct::core::IntegrationType::EULERCT}, {"RK4CT", ct::core::IntegrationType::RK4CT},
+ {"Euler_Sym", ct::core::IntegrationType::EULER_SYM}, {"Rk_Sym", ct::core::IntegrationType::RK_SYM}};
+
+
+ //! mappings for discretization types
+ std::map<APPROXIMATION, std::string> discretizationToString = {{APPROXIMATION::FORWARD_EULER, "Forward_euler"},
+ {APPROXIMATION::BACKWARD_EULER, "Backward_euler"}, {APPROXIMATION::SYMPLECTIC_EULER, "Symplectic_euler"},
+ {APPROXIMATION::TUSTIN, "Tustin"}, {APPROXIMATION::MATRIX_EXPONENTIAL, "Matrix_exponential"}};
+
+ std::map<std::string, APPROXIMATION> stringToDiscretization = {{"Forward_euler", APPROXIMATION::FORWARD_EULER},
+ {"Backward_euler", APPROXIMATION::BACKWARD_EULER}, {"Symplectic_euler", APPROXIMATION::SYMPLECTIC_EULER},
+ {"Tustin", APPROXIMATION::TUSTIN}, {"Matrix_exponential", APPROXIMATION::MATRIX_EXPONENTIAL}};
+
+
+ //! mappings for algorithm types
+ std::map<NLOCP_ALGORITHM, std::string> nlocp_algorithmToString = {{GNMS, "GNMS"}, {ILQR, "ILQR"}};
+
+ std::map<std::string, NLOCP_ALGORITHM> stringTonlocp_algorithm = {{"GNMS", GNMS}, {"ILQR", ILQR}};
+
+
+ //! mappings for linear-quadratic solver types
+ std::map<LQOCP_SOLVER, std::string> locp_solverToString = {
+ {GNRICCATI_SOLVER, "GNRICCATI_SOLVER"}, {HPIPM_SOLVER, "HPIPM_SOLVER"}};
+
+ std::map<std::string, LQOCP_SOLVER> stringTolocp_solver = {
+ {"GNRICCATI_SOLVER", GNRICCATI_SOLVER}, {"HPIPM_SOLVER", HPIPM_SOLVER}};
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp
new file mode 100644
index 0000000..6c7d9ca
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp
@@ -0,0 +1,334 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOptConSolver(const OptConProblem_t& optConProblem,
+ const Settings_t& settings)
+{
+ initialize(optConProblem, settings);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOptConSolver(const OptConProblem_t& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : NLOptConSolver(optConProblem, Settings_t::fromConfigFile(settingsFile, verbose, ns))
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOptConSolver()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initialize(const OptConProblem_t& optConProblem,
+ const Settings_t& settings)
+{
+ if (settings.nThreads > 1)
+ nlocBackend_ = std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(optConProblem, settings));
+ else
+ nlocBackend_ = std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(optConProblem, settings));
+
+ configure(settings);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ if (nlocBackend_->getSettings().nThreads != settings.nThreads)
+ throw std::runtime_error("cannot switch from ST to MT or vice versa. Please call initialize.");
+
+ nlocBackend_->configure(settings);
+
+ switch (settings.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ nlocAlgorithm_ = std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(nlocBackend_, settings));
+ break;
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ nlocAlgorithm_ = std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(nlocBackend_, settings));
+ break;
+ default:
+ throw std::runtime_error("This algorithm is not implemented in NLOptConSolver.hpp");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ nlocAlgorithm_->prepareIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ return nlocAlgorithm_->finishIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ nlocAlgorithm_->prepareMPCIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ return nlocAlgorithm_->finishMPCIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+#ifdef DEBUG_PRINT
+ auto startSolve = std::chrono::steady_clock::now();
+#endif
+ bool success = nlocAlgorithm_->runIteration();
+#ifdef DEBUG_PRINT
+ auto endSolve = std::chrono::steady_clock::now();
+ std::cout << "[NLOC]: runIteration() took "
+ << std::chrono::duration<double, std::milli>(endSolve - startSolve).count() << " ms" << std::endl;
+#endif
+ return success;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ nlocBackend_->setInitialGuess(initialGuess);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::solve()
+{
+ bool foundBetter = true;
+ int numIterations = 0;
+
+ while (foundBetter && (numIterations < nlocBackend_->getSettings().max_iterations))
+ {
+ foundBetter = runIteration();
+
+ numIterations++;
+ }
+
+ return (numIterations > 1 || foundBetter || (numIterations == 1 && !foundBetter));
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Policy_t&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSolution()
+{
+ return nlocBackend_->getSolution();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::StateTrajectory<STATE_DIM, SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateTrajectory() const
+{
+ return nlocBackend_->getStateTrajectory();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::ControlTrajectory<CONTROL_DIM, SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlTrajectory() const
+{
+ return nlocBackend_->getControlTrajectory();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::tpl::TimeArray<SCALAR>& NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeArray() const
+{
+ return nlocBackend_->getTimeArray();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeHorizon() const
+{
+ return nlocBackend_->getTimeHorizon();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeTimeHorizon(const SCALAR& tf)
+{
+ nlocBackend_->changeTimeHorizon(tf);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeInitialState(
+ const core::StateVector<STATE_DIM, SCALAR>& x0)
+{
+ nlocBackend_->changeInitialState(x0);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeCostFunction(
+ const typename OptConProblem_t::CostFunctionPtr_t& cf)
+{
+ nlocBackend_->changeCostFunction(cf);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeNonlinearSystem(
+ const typename OptConProblem_t::DynamicsPtr_t& dyn)
+{
+ nlocBackend_->changeNonlinearSystem(dyn);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeLinearSystem(
+ const typename OptConProblem_t::LinearPtr_t& lin)
+{
+ nlocBackend_->changeLinearSystem(lin);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCost() const
+{
+ return nlocBackend_->getCost();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Settings_t&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSettings()
+{
+ return nlocBackend_->getSettings();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBackend()
+{
+ return nlocBackend_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances()
+{
+ return nlocBackend_->getNonlinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances() const
+{
+ return nlocBackend_->getNonlinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances()
+{
+ return nlocBackend_->getLinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances() const
+{
+ return nlocBackend_->getLinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances()
+{
+ return nlocBackend_->getCostFunctionInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances() const
+{
+ return nlocBackend_->getCostFunctionInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances()
+{
+ return nlocBackend_->getBoxConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances() const
+{
+ return nlocBackend_->getBoxConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances()
+{
+ return nlocBackend_->getGeneralConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances() const
+{
+ return nlocBackend_->getGeneralConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logSummaryToMatlab(const std::string& fileName)
+{
+ nlocBackend_->logSummaryToMatlab(fileName);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp
new file mode 100644
index 0000000..cac1857
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp
@@ -0,0 +1,234 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+#include <ct/optcon/nloc/NLOCBackendST.hpp>
+#include <ct/optcon/nloc/NLOCBackendMP.hpp>
+
+#include <ct/optcon/nloc/algorithms/ilqr/iLQR.hpp>
+#include <ct/optcon/nloc/algorithms/gnms/GNMS.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup OptConSolver OptConSolver
+ * Solver interface for finite horizon optimal control problems
+ */
+template <size_t STATE_DIM,
+ size_t CONTROL_DIM,
+ size_t P_DIM = STATE_DIM / 2,
+ size_t V_DIM = STATE_DIM / 2,
+ typename SCALAR = double>
+class NLOptConSolver : public OptConSolver<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>,
+ core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>,
+ NLOptConSettings,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+ static const size_t POS_DIM = P_DIM;
+ static const size_t VEL_DIM = V_DIM;
+
+ typedef NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Derived;
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ //! constructor
+ NLOptConSolver(const OptConProblem_t& optConProblem, const Settings_t& settings);
+
+ //! constructor
+ NLOptConSolver(const OptConProblem_t& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ //! destructor
+ virtual ~NLOptConSolver();
+
+ /**
+ * configures the solver
+ * */
+ void initialize(const OptConProblem_t& optConProblem, const Settings_t& settings);
+
+ /**
+ * configures the solver
+ * */
+ void configure(const Settings_t& settings) override;
+
+
+ /*!
+ * execute preparation steps for an iteration, e.g. computation of defects
+ */
+ virtual void prepareIteration();
+
+ /*!
+ * execute finishing step for an iteration, e.g. solving Riccati backward pass.
+ * @return
+ */
+ virtual bool finishIteration();
+
+
+ /*!
+ * execute preparation steps for an iteration, e.g. computation of defects
+ */
+ virtual void prepareMPCIteration();
+
+ /*!
+ * execute finishing step for an iteration, e.g. solving Riccati backward pass.
+ * @return
+ */
+ virtual bool finishMPCIteration();
+
+ /**
+ * run a single iteration of the solver
+ * @return true if a better solution was found
+ */
+ virtual bool runIteration();
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ void setInitialGuess(const Policy_t& initialGuess) override;
+
+ /**
+ * solve the optimal control problem
+ * */
+ virtual bool solve() override;
+
+ /**
+ * Get the optimized control policy to the optimal control problem
+ * @return
+ */
+ virtual const Policy_t& getSolution() override;
+
+ /**
+ * Get the optimized trajectory to the optimal control problem
+ * @return
+ */
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const override;
+
+ /**
+ * Get the optimal feedforward control input corresponding to the optimal trajectory
+ * @return
+ */
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const override;
+
+ /**
+ * Get the time indices corresponding to the solution
+ * @return
+ */
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const override;
+
+ /*!
+ * \brief Get the time horizon the solver currently operates on.
+ *
+ */
+ virtual SCALAR getTimeHorizon() const override;
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ virtual void changeTimeHorizon(const SCALAR& tf) override;
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct initial state set
+ */
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) override;
+
+ /*!
+ * \brief Change the cost function
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct cost function
+ */
+ virtual void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf) override;
+
+ /*!
+ * \brief Change the nonlinear system
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct nonlinear system
+ */
+ virtual void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn) override;
+
+ /*!
+ * \brief Change the linear system
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct linear system
+ */
+ virtual void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin) override;
+
+ virtual SCALAR getCost() const override;
+
+ //! get a reference to the current settings
+ const Settings_t& getSettings();
+
+ //! get a reference to the backend (@todo this is not optimal, allows the user too much access)
+ const std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>& getBackend();
+
+ //! get reference to the nonlinear system
+ std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() override;
+
+ //! get constant reference to the nonlinear system
+ const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const override;
+
+ //! get reference to the linearized system
+ std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() override;
+
+ //! get constant reference to the linearized system
+ const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const override;
+
+ //! get reference to the cost function
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() override;
+
+ //! get constant reference to the cost function
+ const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const override;
+
+ //! get reference to the box constraints
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() override;
+
+ //! get constant reference to the boxconstraints
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const override;
+
+ //! get reference to the general constraints
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() override;
+
+ //! get constant reference to the general constraints
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const override;
+
+ //! logging a short summary to matlab
+ void logSummaryToMatlab(const std::string& fileName);
+
+protected:
+ //! the backend holding all the math operations
+ std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>> nlocBackend_;
+
+ //! the algorithm for sequencing the math operations in correct manner
+ std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>> nlocAlgorithm_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/OptConSolver.h b/ct_optcon/include/ct/optcon/solver/OptConSolver.h
new file mode 100644
index 0000000..bf022d0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/OptConSolver.h
@@ -0,0 +1,319 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+/**
+ * OptConSolver.hpp
+ *
+ *
+ * Requirements:
+ * - returns an optimal controller. These can be different controller types, feedforward only, feedforward-feedback, feedback only,
+ * therefore it is templated on the controller type
+ */
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup OptConSolver OptConSolver
+ * Solver interface for finite horizon optimal control problems
+ *
+ * * Requirements:
+ * - returns an optimal controller. These can be different controller types, feedforward only, feedforward-feedback, feedback only,
+ * therefore it is templated on the controller type
+ */
+template <typename DERIVED,
+ typename POLICY,
+ typename SETTINGS,
+ size_t STATE_DIM,
+ size_t CONTROL_DIM,
+ typename SCALAR = double>
+class OptConSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef POLICY Policy_t;
+ typedef SETTINGS Settings_t;
+ typedef DERIVED Derived;
+ typedef SCALAR Scalar_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ OptConSolver() {}
+ virtual ~OptConSolver() {}
+ /*!
+ * \brief Assigns the optimal control problem to the solver.
+ *
+ * Most solvers will require some computational effort to adjust to a new problem.
+ * Therefore, when only adjusting a problem, it is more efficient to adjust individual
+ * properties instead of assigning an entirely new problem. To adjust properties, you can
+ * use
+ * - changeTimeHorizon()
+ * - changeInitialState()
+ * - changeCostFunction()
+ * - changeSystemDynamics()
+ * - changeSystemLinearization()
+ *
+ * @return returns true if the optimal control structure is valid for the given solver
+ */
+ virtual void setProblem(const OptConProblem_t& optConProblem)
+ {
+ optConProblem.verify();
+
+ changeTimeHorizon(optConProblem.getTimeHorizon());
+ changeInitialState(optConProblem.getInitialState());
+ changeCostFunction(optConProblem.getCostFunction());
+ changeNonlinearSystem(optConProblem.getNonlinearSystem());
+ changeLinearSystem(optConProblem.getLinearSystem());
+
+ if (optConProblem.getBoxConstraints())
+ changeBoxConstraints(optConProblem.getBoxConstraints());
+ if (optConProblem.getGeneralConstraints())
+ changeGeneralConstraints(optConProblem.getGeneralConstraints());
+ }
+
+ /**
+ * configures the solver
+ * */
+ virtual void configure(const Settings_t& settings) = 0;
+
+ /**
+ * configures the solver from configFile
+ * */
+ void configureFromFile(const std::string& filename,
+ bool verbose = true,
+ const std::string& ns = DERIVED::SolverName)
+ {
+ Settings_t settings;
+ settings.load(filename, verbose, ns);
+ configure(settings);
+ }
+
+ /**
+ * solve the optimal control problem
+ * @return true if solve succeeded, false otherwise.
+ * */
+ virtual bool solve() = 0;
+
+ /**
+ * run a single iteration of the solver (might not be supported by all solvers)
+ * @return true if a better solution was found
+ */
+ virtual bool runIteration() { throw std::runtime_error("runIteration not supported by solver"); }
+ /**
+ * Get the optimized control policy to the optimal control problem
+ * @return
+ */
+ virtual const Policy_t& getSolution() = 0;
+
+ /**
+ * Get the optimized trajectory to the optimal control problem
+ * @return
+ */
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const = 0;
+
+ /**
+ * Get the optimal feedforward control input corresponding to the optimal trajectory
+ * @return
+ */
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const = 0;
+
+ /**
+ * Get the time indices corresponding to the solution
+ * @return
+ */
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const = 0;
+
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ virtual void setInitialGuess(const Policy_t& initialGuess) = 0;
+
+
+ /*!
+ * \brief Get the time horizon the solver currently operates on.
+ *
+ */
+ virtual SCALAR getTimeHorizon() const = 0;
+
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ virtual void changeTimeHorizon(const SCALAR& tf) = 0;
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct initial state set
+ */
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) = 0;
+
+ /*!
+ * \brief Change the cost function
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct cost function
+ */
+ virtual void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf) = 0;
+
+ /*!
+ * \brief Change the nonlinear system
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct nonlinear system
+ */
+ virtual void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn) = 0;
+
+ /*!
+ * \brief Change the linear system
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct linear system
+ */
+ virtual void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin) = 0;
+
+ /**
+ * @brief Change the box constraints
+ *
+ * This function does not need to be called if
+ * setProblem() has been called with an OptConProblem that
+ * had the correct linear system
+ *
+ * @param[in] con The new box constraints
+ */
+ virtual void changeBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
+ {
+ throw std::runtime_error("The current solver does not support constraints!");
+ }
+
+ /**
+ * @brief Change the general constraints.
+ *
+ * This function does not need to be called if
+ * setProblem() has been called with an OptConProblem that
+ * had the correct linear system
+ *
+ * @param[in] con The new general constraints
+ */
+ virtual void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
+ {
+ throw std::runtime_error("The current solver does not support general constraints!");
+ }
+
+ virtual SCALAR getCost() const { throw std::runtime_error("Get cost not implemented"); }
+ /*!
+ * \brief Direct accessor to the system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeNonlinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const = 0;
+
+ /*!
+ * \brief Direct accessor to the linear system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeLinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const = 0;
+
+ /*!
+ * \brief Direct accessor to the cost function instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const = 0;
+
+ /**
+ * @brief Direct accessor to the box constraint instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The state box constraint instances
+ */
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const = 0;
+
+ /**
+ * @brief Direct accessor to the general constraints
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The general constraints instances.
+ */
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const = 0;
+
+
+ /**
+ * @brief Generates and compiles AD source code which can be used in
+ * the solver. This method can be called just before solving the
+ * problem, i.e. it can be called from the same executable than
+ * the actual solve
+ *
+ * @param[in] problemCG The optcon problem templated on the AD CG Scalar
+ * @param[in] settings The settings indicating what to generate
+ */
+ virtual void generateAndCompileCode(const OptConProblem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>& problemCG,
+ const ct::core::DerivativesCppadSettings& settings)
+ {
+ throw std::runtime_error("Generate and compile code not implemented for this solver");
+ }
+
+ /**
+ * @brief Generates source AD source code which can be used in the
+ * solver. This method needs to be called ahead of actually
+ * solving the problem (e.g. from a different executable)
+ *
+ * @param[in] settings The settings indicating what to generate
+ */
+ virtual void generateCode(const ct::core::DerivativesCppadSettings& settings)
+ {
+ throw std::runtime_error("Generate Code not implemented for this solver");
+ }
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
new file mode 100644
index 0000000..3c59406
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -0,0 +1,354 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
+ : LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>(lqocProblem), N_(-1)
+{
+ Eigen::initParallel();
+ Eigen::setNbThreads(settings_.nThreadsEigen);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::GNRiccatiSolver(int N)
+{
+ changeNumberOfStages(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::~GNRiccatiSolver()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solve()
+{
+ for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
+ solveSingleStage(i);
+
+ computeStateAndControlUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
+{
+ if (N == this->lqocProblem_->getNumberOfStages() - 1)
+ initializeCostToGo();
+
+ designController(N);
+
+ if (N > 0)
+ computeCostToGo(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::configure(const NLOptConSettings& settings)
+{
+ settings_ = settings;
+ H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::StateVectorArray<STATE_DIM, SCALAR> GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSolutionState()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x = p.x_;
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages() + 1; k++)
+ {
+ x[k] += this->lx_[k];
+ }
+
+ return x;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSolutionControl()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u = p.u_;
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ u[k] += this->lu_[k];
+ }
+ return u;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getFeedforwardUpdates()
+{
+ return lv_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getFeedback(
+ ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K)
+{
+ K = L_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStateAndControlUpdates()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ this->delta_x_norm_ = 0.0;
+ this->delta_uff_norm_ = 0.0;
+
+ this->lx_[0].setZero();
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ //! control update rule
+ this->lu_[k] = lv_[k];
+ if (k > 0) // lx is zero for k=0
+ this->lu_[k].noalias() += L_[k] * this->lx_[k];
+
+ //! state update rule
+ this->lx_[k + 1] = p.B_[k] * lv_[k] + p.b_[k];
+ if (k > 0)
+ this->lx_[k + 1].noalias() += (p.A_[k] + p.B_[k] * L_[k]) * this->lx_[k];
+
+ //! compute the norms of the updates
+ //! \todo needed?
+ this->delta_x_norm_ += this->lx_[k + 1].norm();
+ this->delta_uff_norm_ += this->lu_[k].norm();
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSmallestEigenvalue()
+{
+ return smallestEigenvalue_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
+{
+ if (lqocProblem->isConstrained())
+ {
+ throw std::runtime_error(
+ "Selected wrong solver - GNRiccatiSolver cannot handle constrained problems. Use a different solver");
+ }
+
+ const int& N = lqocProblem->getNumberOfStages();
+ changeNumberOfStages(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N)
+{
+ if (N <= 0)
+ return;
+
+ if (N_ == N)
+ return;
+
+ gv_.resize(N);
+ G_.resize(N);
+
+ H_.resize(N);
+ Hi_.resize(N);
+ Hi_inverse_.resize(N);
+
+ lv_.resize(N);
+ L_.resize(N);
+
+ this->lx_.resize(N + 1);
+ this->lu_.resize(N);
+
+ sv_.resize(N + 1);
+ S_.resize(N + 1);
+
+ N_ = N;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
+{
+ //! since intializeCostToGo is the first call, we initialize the smallestEigenvalue here.
+ smallestEigenvalue_ = std::numeric_limits<SCALAR>::infinity();
+
+ // initialize quadratic approximation of cost to go
+ const int& N = this->lqocProblem_->getNumberOfStages();
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ S_[N] = p.Q_[N];
+ sv_[N] = p.qv_[N];
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeCostToGo(size_t k)
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ S_[k] = p.Q_[k];
+ S_[k].noalias() += p.A_[k].transpose() * S_[k + 1] * p.A_[k];
+ S_[k].noalias() -= L_[k].transpose() * Hi_[k] * L_[k];
+
+ S_[k] = 0.5 * (S_[k] + S_[k].transpose()).eval();
+
+ sv_[k] = p.qv_[k];
+ sv_[k].noalias() += p.A_[k].transpose() * sv_[k + 1];
+ sv_[k].noalias() += p.A_[k].transpose() * S_[k + 1] * p.b_[k];
+ sv_[k].noalias() += L_[k].transpose() * Hi_[k] * lv_[k];
+ sv_[k].noalias() += L_[k].transpose() * gv_[k];
+ sv_[k].noalias() += G_[k].transpose() * lv_[k];
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ gv_[k] = p.rv_[k];
+ gv_[k].noalias() += p.B_[k].transpose() * sv_[k + 1];
+ gv_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.b_[k];
+
+ G_[k] = p.P_[k];
+ //G_[k].noalias() += B_[k].transpose() * S_[k+1] * A_[k];
+ G_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.A_[k];
+
+ H_[k] = p.R_[k];
+ //H_[k].noalias() += B_[k].transpose() * S_[k+1] * B_[k];
+ H_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.B_[k];
+
+ if (settings_.fixedHessianCorrection)
+ {
+ if (settings_.epsilon > 1e-10)
+ Hi_[k] = H_[k] + settings_.epsilon * ControlMatrix::Identity();
+ else
+ Hi_[k] = H_[k];
+
+ if (settings_.recordSmallestEigenvalue)
+ {
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(Hi_[k], Eigen::ComputeEigenvectors);
+ const ControlMatrix& V = eigenvalueSolver_.eigenvectors().real();
+ const ControlVector& lambda = eigenvalueSolver_.eigenvalues();
+
+ smallestEigenvalue_ = std::min(smallestEigenvalue_, lambda.minCoeff());
+
+ // Corrected Eigenvalue Matrix
+ ControlMatrix D = ControlMatrix::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(settings_.epsilon);
+
+ // reconstruct H
+ ControlMatrix Hi_regular = V * D * V.transpose();
+
+ // invert D
+ ControlMatrix D_inverse = ControlMatrix::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = -1.0 * D.diagonal().cwiseInverse();
+ ControlMatrix Hi_inverse_regular = V * D_inverse * V.transpose();
+
+ if (!Hi_inverse_[k].isApprox(Hi_inverse_regular, 1e-4))
+ {
+ std::cout << "warning, inverses not identical at " << k << std::endl;
+ std::cout << "Hi_inverse_fixed - Hi_inverse_regular: " << std::endl
+ << Hi_inverse_[k] - Hi_inverse_regular << std::endl
+ << std::endl;
+ }
+ }
+
+ Hi_inverse_[k] = -Hi_[k].template selfadjointView<Eigen::Lower>().llt().solve(ControlMatrix::Identity());
+
+ // calculate FB gain update
+ L_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * G_[k];
+
+ // calculate FF update
+ lv_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * gv_[k];
+ }
+ else
+ {
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(H_[k], Eigen::ComputeEigenvectors);
+ const ControlMatrix& V = eigenvalueSolver_.eigenvectors().real();
+ const ControlVector& lambda = eigenvalueSolver_.eigenvalues();
+
+ if (settings_.recordSmallestEigenvalue)
+ {
+ smallestEigenvalue_ = std::min(smallestEigenvalue_, lambda.minCoeff());
+ }
+
+ // Corrected Eigenvalue Matrix
+ ControlMatrix D = ControlMatrix::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(settings_.epsilon);
+
+ // reconstruct H
+ Hi_[k].noalias() = V * D * V.transpose();
+
+ // invert D
+ ControlMatrix D_inverse = ControlMatrix::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = -1.0 * D.diagonal().cwiseInverse();
+ Hi_inverse_[k].noalias() = V * D_inverse * V.transpose();
+
+ // calculate FB gain update
+ L_[k].noalias() = Hi_inverse_[k] * G_[k];
+
+ // calculate FF update
+ lv_[k].noalias() = Hi_inverse_[k] * gv_[k];
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
+{
+#ifdef MATLAB_FULL_LOG
+
+ matFile_.open("GNRiccatiSolver.mat");
+
+ matFile_.put("sv", sv_.toImplementation());
+ matFile_.put("S", S_.toImplementation());
+ matFile_.put("L", L_.toImplementation());
+ matFile_.put("H", H_.toImplementation());
+ matFile_.put("Hi_", Hi_.toImplementation());
+ matFile_.put("Hi_inverse", Hi_inverse_.toImplementation());
+ matFile_.put("G", G_.toImplementation());
+ matFile_.put("gv", gv_.toImplementation());
+
+ matFile_.close();
+#endif
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeAndAllocate()
+{
+ // do nothing
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
new file mode 100644
index 0000000..66550e6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LQOCSolver.hpp"
+
+#ifdef MATLAB_FULL_LOG
+#include <ct/optcon/matlab.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * This class implements an general Riccati backward pass for solving an unconstrained
+ * linear-quadratic Optimal Control problem
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class GNRiccatiSolver : public LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const int state_dim = STATE_DIM;
+ static const int control_dim = CONTROL_DIM;
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+
+ typedef ct::core::StateMatrix<STATE_DIM, SCALAR> StateMatrix;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> StateMatrixArray;
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> ControlVector;
+ typedef ct::core::ControlMatrix<CONTROL_DIM, SCALAR> ControlMatrix;
+ typedef ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> ControlMatrixArray;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> StateControlMatrixArray;
+ typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> FeedbackArray;
+
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+
+ GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr);
+
+ GNRiccatiSolver(int N);
+
+ virtual ~GNRiccatiSolver();
+
+ virtual void solve() override;
+
+ virtual void initializeAndAllocate() override;
+
+ virtual void solveSingleStage(int N) override;
+
+ virtual void configure(const NLOptConSettings& settings) override;
+
+ virtual ct::core::StateVectorArray<STATE_DIM, SCALAR> getSolutionState() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getSolutionControl() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getFeedforwardUpdates() override;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K) override;
+
+ //! compute the state and control updates.
+ /*!
+ * this method is specific to the GN Riccati solver, since the state updates lx_
+ * need to be completed in an additional forward sweep.
+ *
+ * IMPORTANT: you need to call this method at the right place if you're using solveSingleStage() by yourself.
+ */
+ virtual void computeStateAndControlUpdates() override;
+
+ virtual SCALAR getSmallestEigenvalue() override;
+
+protected:
+ /*!
+ * resize matrices
+ * @param lqocProblem
+ */
+ virtual void setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
+
+ void changeNumberOfStages(int N);
+
+ void initializeCostToGo();
+
+ void computeCostToGo(size_t k);
+
+ void designController(size_t k);
+
+ void logToMatlab();
+
+ NLOptConSettings settings_;
+
+ ControlVectorArray gv_;
+ FeedbackArray G_;
+
+ ControlMatrixArray H_;
+ ControlMatrixArray Hi_;
+ ControlMatrixArray Hi_inverse_;
+ ControlMatrix H_corrFix_;
+
+ ControlVectorArray lv_;
+ FeedbackArray L_;
+
+ StateVectorArray sv_;
+ StateMatrixArray S_;
+
+ int N_;
+
+ SCALAR smallestEigenvalue_;
+
+ //! Eigenvalue solver, used for inverting the Hessian and for regularization
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM>> eigenvalueSolver_;
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB_FULL_LOG
+ matlab::MatFile matFile_;
+#endif //MATLAB
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
new file mode 100644
index 0000000..01255c0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -0,0 +1,703 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef HPIPM
+
+namespace ct {
+namespace optcon {
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+HPIPMInterface<STATE_DIM, CONTROL_DIM>::HPIPMInterface(int N) : N_(-1), x0_(nullptr), settings_(NLOptConSettings())
+{
+ // some zero variables
+ hb0_.setZero();
+ hr0_.setZero();
+
+ // by default, set number of box and general constraints to zero
+ nb_.resize(1, 0);
+ ng_.resize(1, 0);
+
+ configure(settings_);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+HPIPMInterface<STATE_DIM, CONTROL_DIM>::~HPIPMInterface()
+{
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::initializeAndAllocate()
+{
+ int qp_size = ::d_memsize_ocp_qp(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data());
+ qp_mem_.resize(qp_size);
+ ::d_create_ocp_qp(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data(), &qp_, qp_mem_.data());
+
+ int qp_sol_size = ::d_memsize_ocp_qp_sol(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data());
+ qp_sol_mem_.resize(qp_sol_size);
+ ::d_create_ocp_qp_sol(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data(), &qp_sol_, qp_sol_mem_.data());
+
+ int ipm_size = ::d_memsize_ipm_hard_ocp_qp(&qp_, &arg_);
+ ipm_mem_.resize(ipm_size);
+ ::d_create_ipm_hard_ocp_qp(&qp_, &arg_, &workspace_, ipm_mem_.data());
+
+ std::cout << "HPIPM allocating memory for QP"
+ << std::endl; // always print to make sure users take note in case of wrong use
+ if (settings_.lqoc_solver_settings.lqoc_debug_print)
+ {
+ std::cout << "HPIPM qp_size: " << qp_size << std::endl;
+ std::cout << "HPIPM qp_sol_size: " << qp_sol_size << std::endl;
+ std::cout << "HPIPM ipm_size: " << ipm_size << std::endl;
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configure(const NLOptConSettings& settings)
+{
+ settings_ = settings;
+
+ arg_.iter_max = settings_.lqoc_solver_settings.num_lqoc_iterations;
+
+ arg_.alpha_min = 1e-8; // todo review and make setting
+ arg_.mu_max = 1e-12; // todo review and make setting
+ arg_.mu0 = 2.0; // todo review and make setting
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::solve()
+{
+ // optional printout
+ // for (size_t i = 0; i < N_ + 1; i++)
+ // {
+ // std::cout << "HPIPM matrix printout for stage " << i << std::endl;
+ // if (i<N_)
+ // {
+ // printf("\nA\n");
+ // d_print_mat(STATE_DIM, STATE_DIM, hA_[i], STATE_DIM);
+ // printf("\nB\n");
+ // d_print_mat(STATE_DIM, CONTROL_DIM, hB_[i], STATE_DIM);
+ // printf("\nb\n");
+ // d_print_mat(1, STATE_DIM, hb_[i], 1);
+ // }
+ //
+ // printf("\nQ\n");
+ // d_print_mat(STATE_DIM, STATE_DIM, hQ_[i], STATE_DIM);
+ // printf("\nq\n");
+ // d_print_mat(1, STATE_DIM, hq_[i], 1);
+ //
+ //
+ // if (i<N_)
+ // {
+ // printf("\nR\n");
+ // d_print_mat(CONTROL_DIM, CONTROL_DIM, hR_[i], CONTROL_DIM);
+ // printf("\nS\n");
+ // d_print_mat(CONTROL_DIM, STATE_DIM, hS_[i], CONTROL_DIM);
+ // printf("\nr\n");
+ // d_print_mat(1, CONTROL_DIM, hr_[i], 1);
+ // }
+ //
+ // int_print_mat(1, nb_[i], hidxb_[i], 1);
+ // printf("\nhd_lb_\n");
+ // d_print_mat(1, nb_[i], hd_lb_[i], 1);
+ // printf("\nhd_ub_\n");
+ // d_print_mat(1, nb_[i], hd_ub_[i], 1);
+ // } // end optional printout
+
+ // set pointers to optimal control problem
+ ::d_cvt_colmaj_to_ocp_qp(hA_.data(), hB_.data(), hb_.data(), hQ_.data(), hS_.data(), hR_.data(), hq_.data(),
+ hr_.data(), hidxb_.data(), hd_lb_.data(), hd_ub_.data(), hC_.data(), hD_.data(), hd_lg_.data(), hd_ug_.data(),
+ &qp_);
+
+ // solve optimal control problem
+ ::d_solve_ipm2_hard_ocp_qp(&qp_, &qp_sol_, &workspace_);
+
+ // display iteration summary
+ if (settings_.lqoc_solver_settings.lqoc_debug_print)
+ {
+ printf("\nipm iter = %d\n", workspace_.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, workspace_.iter, workspace_.stat, 5);
+ }
+
+ // extract state and control updates
+ computeStateAndControlUpdates();
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeStateAndControlUpdates()
+{
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+
+ // convert optimal control problem solution to standard column-major representation
+ ::d_cvt_ocp_qp_sol_to_colmaj(&qp_, &qp_sol_, u_.data(), x_.data(), pi_.data(), lam_lb_.data(), lam_ub_.data(),
+ lam_lg_.data(), lam_ug_.data());
+
+ hx_[0] = this->lqocProblem_->x_[0];
+
+ this->delta_x_norm_ = 0.0;
+ this->delta_uff_norm_ = 0.0;
+
+ this->lx_[0].setZero();
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ // reconstruct control update
+ this->lu_[k] = hu_[k] - p.u_[k];
+
+ // reconstruct state update
+ this->lx_[k + 1] = hx_[k + 1] - p.x_[k + 1];
+
+ // compute the norms of the updates
+ // TODO needed?
+ this->delta_x_norm_ += this->lx_[k + 1].norm();
+ this->delta_uff_norm_ += this->lu_[k].norm();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::StateVectorArray<STATE_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getSolutionState()
+{
+ return hx_;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::ControlVectorArray<CONTROL_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getSolutionControl()
+{
+ return hu_;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>& K)
+{
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+ K.resize(p.getNumberOfStages());
+
+ // for stage 0, HPIPM does not provide feedback, so we have to construct it
+
+ // step 1: reconstruct H[0]
+ Eigen::Matrix<double, control_dim, control_dim> Lr;
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[0], 0, 0, Lr.data(), Lr.rows());
+ Eigen::Matrix<double, control_dim, control_dim> H;
+ H = Lr.template triangularView<Eigen::Lower>() * Lr.transpose(); // Lr is cholesky of H
+
+ // step2: reconstruct S[1]
+ Eigen::Matrix<double, state_dim, state_dim> Lq;
+ ::d_cvt_strmat2mat(Lq.rows(), Lq.cols(), &workspace_.L[1], control_dim, control_dim, Lq.data(), Lq.rows());
+ Eigen::Matrix<double, state_dim, state_dim> S;
+ S = Lq.template triangularView<Eigen::Lower>() * Lq.transpose(); // Lq is cholesky of S
+
+ // step3: compute G[0]
+ Eigen::Matrix<double, control_dim, state_dim> G;
+ G = p.P_[0];
+ G.noalias() += p.B_[0].transpose() * S * p.A_[0];
+
+ // step4: compute K[0]
+ K[0] = (-H.inverse() * G); // \todo use Lr here instead of H!
+
+
+ // for all other steps we can just read Ls
+ Eigen::Matrix<double, state_dim, control_dim> Ls;
+ for (int i = 1; i < this->lqocProblem_->getNumberOfStages(); i++)
+ {
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[i], 0, 0, Lr.data(), Lr.rows());
+ ::d_cvt_strmat2mat(Ls.rows(), Ls.cols(), &workspace_.L[i], Lr.rows(), 0, Ls.data(), Ls.rows());
+ K[i] = (-Ls * Lr.partialPivLu().inverse()).transpose();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::ControlVectorArray<CONTROL_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getFeedforwardUpdates()
+{
+ throw std::runtime_error("HPIPMInterface: getFeedforwardUpdates Not implemented");
+
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+ ct::core::ControlVectorArray<CONTROL_DIM> lv(p.getNumberOfStages());
+
+ for (int i = 1; i < this->lqocProblem_->getNumberOfStages(); i++)
+ {
+ Eigen::Matrix<double, control_dim, control_dim> Lr;
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[i], 0, 0, Lr.data(), Lr.rows());
+
+ Eigen::Matrix<double, 1, control_dim> llTranspose;
+ ::d_cvt_strmat2mat(llTranspose.rows(), llTranspose.cols(), &workspace_.L[i], control_dim + state_dim, 0,
+ llTranspose.data(), llTranspose.rows());
+
+ lv[i] = -Lr.transpose().inverse() * llTranspose.transpose();
+ }
+
+ return lv;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::printSolution()
+{
+ int ii;
+
+ ::d_cvt_ocp_qp_sol_to_colmaj(&qp_, &qp_sol_, u_.data(), x_.data(), pi_.data(), lam_lb_.data(), lam_ub_.data(),
+ lam_lg_.data(), lam_ug_.data());
+
+ printf("\nsolution\n\n");
+ printf("\nu\n");
+ for (ii = 0; ii <= N_; ii++)
+ d_print_mat(1, nu_[ii], u_[ii], 1);
+ printf("\nx\n");
+ for (ii = 0; ii <= N_; ii++)
+ d_print_mat(1, nx_[ii], x_[ii], 1);
+
+ /*
+ #if 1
+ printf("\npi\n");
+ for(ii=0; ii<N_; ii++)
+ d_print_mat(1, nx_[ii+1], pi[ii], 1);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], lam_lb[ii], 1);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], lam_ub[ii], 1);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], lam_lg[ii], 1);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], lam_ug[ii], 1);
+
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], (qp_sol_.t_lb+ii)->pa, 1);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], (qp_sol_.t_ub+ii)->pa, 1);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], (qp_sol_.t_lg+ii)->pa, 1);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], (qp_sol_.t_ug+ii)->pa, 1);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nu_[ii]+nx_[ii], (workspace_.res_g+ii)->pa, 1);
+ printf("\nres_b\n");
+ for(ii=0; ii<N_; ii++)
+ d_print_e_mat(1, nx_[ii+1], (workspace_.res_b+ii)->pa, 1);
+ printf("\nres_m_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_m_lb+ii)->pa, 1);
+ printf("\nres_m_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_m_ub+ii)->pa, 1);
+ printf("\nres_m_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_m_lg+ii)->pa, 1);
+ printf("\nres_m_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_m_ug+ii)->pa, 1);
+ printf("\nres_d_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_d_lb+ii)->pa, 1);
+ printf("\nres_d_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_d_ub+ii)->pa, 1);
+ printf("\nres_d_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_d_lg+ii)->pa, 1);
+ printf("\nres_d_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_d_ug+ii)->pa, 1);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace_.res_mu);
+ #endif
+ */
+
+ printf("\nipm iter = %d\n", workspace_.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ ::d_print_e_tran_mat(5, workspace_.iter, workspace_.stat, 5);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setProblemImpl(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ // check if the number of stages N changed and adapt problem dimensions
+ bool nStagesChanged = changeNumberOfStages(lqocProblem->getNumberOfStages());
+
+
+ // WARNING: the allocation should in practice not have to happen during the loop.
+ // If possible, prefer receding horizon MPC problems.
+ // If the number of stages has changed, however, the problem needs to be re-built:
+ if (nStagesChanged)
+ {
+ // update constraint configuration in case the horizon length has changed.
+ if (lqocProblem->isBoxConstrained())
+ configureBoxConstraints(lqocProblem);
+
+ if (lqocProblem->isGeneralConstrained())
+ configureGeneralConstraints(lqocProblem);
+ }
+
+ // we do not need to reset the pointers if
+ bool keepPointers = this->lqocProblem_ && //there was an lqocProblem before
+ N_ == lqocProblem->getNumberOfStages() && // and the number of states did not change
+ this->lqocProblem_ == lqocProblem; // and it was the same pointer
+
+ // setup unconstrained part of problem
+ setupCostAndDynamics(lqocProblem->x_, lqocProblem->u_, lqocProblem->A_, lqocProblem->B_, lqocProblem->b_,
+ lqocProblem->P_, lqocProblem->qv_, lqocProblem->Q_, lqocProblem->rv_, lqocProblem->R_, keepPointers);
+
+
+ if (nStagesChanged)
+ {
+ initializeAndAllocate();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureBoxConstraints(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ // stages 1 to N
+ for (size_t i = 0; i < N_ + 1; i++)
+ {
+ nb_[i] = lqocProblem->nb_[i];
+
+ // set pointers to box constraint boundaries and sparsity pattern
+ hd_lb_[i] = lqocProblem->ux_lb_[i].data();
+ hd_ub_[i] = lqocProblem->ux_ub_[i].data();
+ hidxb_[i] = lqocProblem->ux_I_[i].data();
+
+ // first stage requires special treatment as state is not a decision variable
+ if (i == 0)
+ {
+ nb_[i] = 0;
+ for (int j = 0; j < lqocProblem->nb_[i]; j++)
+ {
+ if (lqocProblem->ux_I_[i](j) < CONTROL_DIM)
+ nb_[i]++; // adapt number of constraints such that only controls are listed as decision vars
+ else
+ break;
+ }
+ }
+
+ // TODO clarify with Gianluca if we need to reset the lagrange multiplier
+ // before warmstarting (potentially wrong warmstart for the lambdas)
+
+ // direct pointers of lagrange mult to corresponding containers
+ lam_lb_[i] = cont_lam_lb_[i].data();
+ lam_ub_[i] = cont_lam_ub_[i].data();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureGeneralConstraints(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ for (size_t i = 0; i < N_ + 1; i++)
+ {
+ // check dimensions
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->d_ub_[i].rows());
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->C_[i].rows());
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->D_[i].rows());
+ assert(lqocProblem->C_[i].cols() == STATE_DIM);
+ assert(lqocProblem->D_[i].cols() == CONTROL_DIM);
+
+ // get the number of constraints
+ ng_[i] = lqocProblem->d_lb_[i].rows();
+
+ // set pointers to hpipm-style box constraint boundaries and sparsity pattern
+ hd_lg_[i] = lqocProblem->d_lb_[i].data();
+ hd_ug_[i] = lqocProblem->d_ub_[i].data();
+ hC_[i] = lqocProblem->C_[i].data();
+ hD_[i] = lqocProblem->D_[i].data();
+
+ // TODO clarify with Gianluca if we need to reset the lagrange multiplier
+ // before warmstarting (potentially wrong warmstart for the lambdas)
+
+ // direct pointers of lagrange mult to corresponding containers
+ cont_lam_lg_[i].resize(ng_[i]); // todo avoid dynamic allocation (e.g. by defining a max. constraint dim)
+ cont_lam_ug_[i].resize(ng_[i]); // todo avoid dynamic allocation (e.g. by defining a max. constraint dim)
+ lam_lg_[i] = cont_lam_lg_[i].data();
+ lam_ug_[i] = cont_lam_ug_[i].data();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setupCostAndDynamics(StateVectorArray& x,
+ ControlVectorArray& u,
+ StateMatrixArray& A,
+ StateControlMatrixArray& B,
+ StateVectorArray& b,
+ FeedbackArray& P,
+ StateVectorArray& qv,
+ StateMatrixArray& Q,
+ ControlVectorArray& rv,
+ ControlMatrixArray& R,
+ bool keepPointers)
+{
+ if (N_ == -1)
+ throw std::runtime_error("Time horizon not set, please set it first");
+
+ // set the initial state
+ x0_ = x[0].data();
+
+ /*
+ * transcribe the "differential" representation of the OptConProblem to the absolute origin of
+ * the linear system.
+ * Note: constant terms are not even handed over above (not important for solving LQ problem).
+ */
+
+ // STEP 1: transcription of affine system dynamics offset term
+ for (int i = 0; i < N_; i++)
+ {
+ bEigen_[i] = b[i] + x[i + 1] - A[i] * x[i] - B[i] * u[i];
+ }
+ hb0_ = b[0] + x[1] - B[0] * u[0]; // this line needs to be transcribed separately (correction for first stage)
+
+
+ // STEP 2: transcription of intermediate costs
+ for (int i = 0; i < N_; i++)
+ {
+ hqEigen_[i] = qv[i] - Q[i] * x[i] - P[i].transpose() * u[i];
+ hrEigen_[i] = rv[i] - R[i] * u[i] - P[i] * x[i];
+ }
+ hr0_ = hrEigen_[0] + P[0] * x[0]; // this line needs to be transcribed separately (correction for first stage)
+
+
+ // STEP 3: transcription of terminal cost terms
+ hqEigen_[N_] = qv[N_] - Q[N_] * x[N_];
+
+
+ // STEP 4: The following quantities remain unchanged when changing coordinate systems
+ if (!keepPointers)
+ {
+ for (int i = 0; i < N_; i++)
+ {
+ hA_[i] = A[i].data();
+ hB_[i] = B[i].data();
+ }
+
+ // intermediate cost hessians and cross-terms
+ for (int i = 0; i < N_; i++)
+ {
+ hQ_[i] = Q[i].data();
+ hS_[i] = P[i].data();
+ hR_[i] = R[i].data();
+ }
+
+ // final cost hessian state
+ hQ_[N_] = Q[N_].data();
+ }
+
+ // reset lqocProblem pointer, will get set in Base class if needed
+ this->lqocProblem_ = nullptr;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::changeNumberOfStages(int N)
+{
+ if (N_ == N)
+ return false; // return since problem is already correctly sized
+
+ N_ = N;
+
+ this->lx_.resize(N + 1);
+ this->lu_.resize(N);
+
+ nx_.resize(N_ + 1, STATE_DIM); // initialize number of states per stage
+ nu_.resize(N_ + 1, CONTROL_DIM); // initialize number of control inputs per stage
+ nb_.resize(N_ + 1, nb_.back()); // initialize number of box constraints per stage
+ ng_.resize(N_ + 1, ng_.back()); // initialize number of general constraints per stage
+
+ // resize the containers for the affine system dynamics approximation
+ hA_.resize(N_);
+ hB_.resize(N_);
+ bEigen_.resize(N_);
+ hb_.resize(N_);
+
+ // resize the containers for the LQ cost approximation
+ hQ_.resize(N_ + 1);
+ hS_.resize(N_ + 1);
+ hR_.resize(N_ + 1);
+ hqEigen_.resize(N_ + 1);
+ hq_.resize(N_ + 1);
+ hrEigen_.resize(N_ + 1);
+ hr_.resize(N_ + 1);
+
+ hd_lb_.resize(N_ + 1);
+ hd_ub_.resize(N_ + 1);
+ hidxb_.resize(N_ + 1);
+ hd_lg_.resize(N_ + 1);
+ hd_ug_.resize(N_ + 1);
+ hC_.resize(N_ + 1);
+ hD_.resize(N_ + 1);
+
+ u_.resize(N_ + 1);
+ x_.resize(N_ + 1);
+ pi_.resize(N_);
+ hx_.resize(N_ + 1);
+ hpi_.resize(N_);
+ hu_.resize(N_);
+
+ lam_lb_.resize(N_ + 1);
+ lam_ub_.resize(N_ + 1);
+ lam_lg_.resize(N_ + 1);
+ lam_ug_.resize(N_ + 1);
+ cont_lam_lb_.resize(N_ + 1);
+ cont_lam_ub_.resize(N_ + 1);
+ cont_lam_lg_.resize(N_ + 1);
+ cont_lam_ug_.resize(N_ + 1);
+
+ for (int i = 0; i < N_; i++)
+ {
+ // first state and last input are not optimized
+ x_[i + 1] = hx_[i + 1].data();
+ u_[i] = hu_[i].data();
+ }
+ for (int i = 0; i < N_; i++)
+ {
+ pi_[i] = hpi_[i].data();
+ }
+ for (int i = 0; i < N_; i++)
+ {
+ hq_[i] = hqEigen_[i].data();
+ hr_[i] = hrEigen_[i].data();
+ hb_[i] = bEigen_[i].data();
+ }
+
+ hS_[N_] = nullptr;
+ hR_[N_] = nullptr;
+
+ hq_[N_] = hqEigen_[N_].data();
+ hr_[N_] = nullptr;
+
+ hb_[0] = hb0_.data();
+ hr_[0] = hr0_.data();
+
+ ct::core::StateVectorArray<STATE_DIM> hx;
+ ct::core::ControlVectorArray<CONTROL_DIM> hu;
+
+ // initial state is not a decision variable but given
+ nx_[0] = 0;
+
+ // last input is not a decision variable
+ nu_[N] = 0;
+
+ return true;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_zeros(double** pA, int row, int col)
+{
+ *pA = (double*)malloc((row * col) * sizeof(double));
+ double* A = *pA;
+ int i;
+ for (i = 0; i < row * col; i++)
+ A[i] = 0.0;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_mat(int m, int n, double* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < m; i++)
+ {
+ for (j = 0; j < n; j++)
+ {
+ printf("%9.5f ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_tran_mat(int row, int col, double* A, int lda)
+{
+ int i, j;
+ for (j = 0; j < col; j++)
+ {
+ for (i = 0; i < row; i++)
+ {
+ printf("%9.5f ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_e_mat(int m, int n, double* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < m; i++)
+ {
+ for (j = 0; j < n; j++)
+ {
+ printf("%e\t", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_e_tran_mat(int row, int col, double* A, int lda)
+{
+ int i, j;
+ for (j = 0; j < col; j++)
+ {
+ for (i = 0; i < row; i++)
+ {
+ printf("%e\t", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::int_print_mat(int row, int col, int* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < row; i++)
+ {
+ for (j = 0; j < col; j++)
+ {
+ printf("%d ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+} // namespace optcon
+} // namespace ct
+
+#endif // HPIPM
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
new file mode 100644
index 0000000..ac1a403
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
@@ -0,0 +1,292 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LQOCSolver.hpp"
+
+#ifdef HPIPM
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+extern "C" {
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_sol.h>
+#include <hpipm_d_ocp_qp_ipm_hard.h>
+}
+
+#include <unsupported/Eigen/MatrixFunctions>
+
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * This class implements an interface to the HPIPM solver
+ *
+ * \warning in order to allow for an efficient implementation of constrained MPC,
+ * the configuration of the box and general constraints must be done independently
+ * from setProblem()
+ */
+template <int STATE_DIM, int CONTROL_DIM>
+class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const int state_dim = STATE_DIM;
+ static const int control_dim = CONTROL_DIM;
+ static const int max_box_constr_dim = STATE_DIM + CONTROL_DIM;
+
+ using StateMatrix = ct::core::StateMatrix<STATE_DIM>;
+ using StateMatrixArray = ct::core::StateMatrixArray<STATE_DIM>;
+ using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM>;
+ using ControlMatrixArray = ct::core::ControlMatrixArray<CONTROL_DIM>;
+ using StateControlMatrixArray = ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM>;
+ using FeedbackArray = ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>;
+
+ using StateVectorArray = ct::core::StateVectorArray<STATE_DIM>;
+ using ControlVectorArray = ct::core::ControlVectorArray<CONTROL_DIM>;
+
+ // definitions for variable-size constraints
+ using constr_vec_t = Eigen::Matrix<double, -1, 1>;
+ using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;
+
+
+ //! typedef a container for a sparsity pattern vector for box constraints
+ using box_constr_sparsity_t = Eigen::Matrix<int, max_box_constr_dim, 1>;
+
+ //! constructor
+ HPIPMInterface(int N = -1);
+
+ //! destructor
+ virtual ~HPIPMInterface();
+
+ virtual void configure(const NLOptConSettings& settings) override;
+
+ void solve() override;
+
+ virtual void computeStateAndControlUpdates() override;
+
+ virtual ct::core::StateVectorArray<STATE_DIM> getSolutionState() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM> getSolutionControl() override;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>& K) override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM> getFeedforwardUpdates() override;
+
+ void printSolution();
+
+ //! brief setup and configure the box constraints
+ virtual void configureBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ //! brief setup and configure the general (in)equality constraints
+ virtual void configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ /*!
+ * @brief allocate memory for HPIPM
+ *
+ * \warning These functions are assumed to be used only outside the control loop.
+ * Their intended use is to call them before the loop to setup the memory needed by
+ * the QP structures and solver (dynamic memory allocation is time consuming).
+ *
+ * Needs to be called when
+ * - the number of stages changes
+ * - the box constraint configuration changes
+ * - the general constraint configuration changes
+ */
+ virtual void initializeAndAllocate() override;
+
+private:
+ /*!
+ * @brief set problem implementation for hpipm
+ * \warning This method is called in the loop. As little memory as possible
+ * should be allocated in this function. Ideally this method only sets pointers.
+ *
+ * \warning If you wish to
+ */
+ void setProblemImpl(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ /*!
+ * @brief transcribe the problem from original local formulation to HPIPM's global coordinates
+ *
+ * See also the description of the LQOC Problem in class LQOCProblem.h
+ *
+ * @param x current state reference \f$ \hat \mathbf x_n \f$
+ * @param u current control reference \f$ \hat \mathbf u_n \f$
+ * @param A affine system matrices \f$ \mathbf A_n \f$
+ * @param B affine system matrices \f$ \mathbf B_n \f$
+ * @param b affine system matrix \f$ \mathbf b_n \f$
+ * @param P mixed cost term \f$ \mathbf P_n \f$
+ * @param qv pure state-cost term \f$ \mathbf q_n \f$ (first order derivative)
+ * @param Q pure state-cost term \f$ \mathbf Q_n \f$ (second order derivative)
+ * @param rv pure input-cost term \f$ \mathbf r_n \f$ (first order derivative)
+ * @param R pure input-cost term \f$ \mathbf R_n \f$ (second order derivative)
+ * @param keepPointers keep pointers
+ *
+ *
+ * This method needs change coordinate systems, in the sense that
+ * \f[
+ * \mathbf x_{n+1} = \mathbf A_n \mathbf x_n + \mathbf B_n \mathbf u_n +\mathbf b_n
+ * + \hat \mathbf x_{n+1} - \mathbf A_n \hat \mathbf x_n - \mathbf B_n \hat \mathbf u_n
+ * \f]
+ */
+ void setupCostAndDynamics(StateVectorArray& x,
+ ControlVectorArray& u,
+ StateMatrixArray& A,
+ StateControlMatrixArray& B,
+ StateVectorArray& b,
+ FeedbackArray& P,
+ StateVectorArray& qv,
+ StateMatrixArray& Q,
+ ControlVectorArray& rv,
+ ControlMatrixArray& R,
+ bool keepPointers = false);
+
+ /*!
+ * @brief change number of states of the optimal control problem
+ * @return true if number of stages changed, false if number of stages is unchanged.
+ */
+ bool changeNumberOfStages(int N);
+
+ //! creates a zero matrix
+ void d_zeros(double** pA, int row, int col);
+
+ //! prints a matrix in column-major format
+ void d_print_mat(int m, int n, double* A, int lda);
+
+ //! prints a matrix in column-major format (exponential notation)
+ void d_print_e_mat(int m, int n, double* A, int lda);
+
+ //! prints the transposed of a matrix in column-major format (exponential notation)
+ void d_print_e_tran_mat(int row, int col, double* A, int lda);
+
+ //! prints the transposed of a matrix in column-major format
+ void d_print_tran_mat(int row, int col, double* A, int lda);
+
+ //! prints a matrix in column-major format
+ void int_print_mat(int row, int col, int* A, int lda);
+
+ //! horizon length
+ int N_;
+
+ //! number of states per stage
+ std::vector<int> nx_;
+ //! number of inputs per stage
+ std::vector<int> nu_;
+
+ //! number of box constraints per stage
+ std::vector<int> nb_;
+ //! number of general constraints per stage
+ std::vector<int> ng_;
+
+ //! initial state
+ double* x0_;
+
+ //! system state sensitivities
+ std::vector<double*> hA_;
+ //! system input sensitivities
+ std::vector<double*> hB_;
+ //! system offset term
+ std::vector<double*> hb_;
+ //! intermediate container for intuitive transcription of system representation from local to global coordinates
+ StateVectorArray bEigen_;
+ //! intermediate container for intuitive transcription of first stage from local to global coordinates
+ Eigen::Matrix<double, state_dim, 1> hb0_;
+
+
+ //! pure state penalty hessian
+ std::vector<double*> hQ_;
+ //! state-control cross-terms
+ std::vector<double*> hS_;
+ //! pure control penalty hessian
+ std::vector<double*> hR_;
+ //! pure state penalty jacobian
+ std::vector<double*> hq_;
+ //! pure control penalty jacobian
+ std::vector<double*> hr_;
+ //! intermediate container for intuitive transcription of first stage from local to global coordinates
+ Eigen::Matrix<double, control_dim, 1> hr0_;
+ //! interm. container for intuitive transcription of 1st order state penalty from local to global coordinates
+ StateVectorArray hqEigen_;
+ //! interm. container for intuitive transcription of 1st order control penalty from local to global coordinates
+ ControlVectorArray hrEigen_;
+
+
+ //! pointer to lower box constraint boundary
+ std::vector<double*> hd_lb_;
+ //! pointer to upper box constraint boundary
+ std::vector<double*> hd_ub_;
+ //! pointer to sparsity pattern for box constraints
+ std::vector<int*> hidxb_;
+
+ //! lower general constraint boundary
+ std::vector<double*> hd_lg_;
+ //! upper general constraint boundary
+ std::vector<double*> hd_ug_;
+ //! general constraint jacobians w.r.t. states
+ std::vector<double*> hC_;
+ //! general constraint jacobians w.r.t. controls (presumably)
+ std::vector<double*> hD_;
+
+
+ /*
+ * SOLUTION variables
+ */
+ //! optimal control trajectory
+ std::vector<double*> u_;
+ //! optimal state trajectory
+ std::vector<double*> x_;
+ //! @todo what is this ?
+ std::vector<double*> pi_;
+ //! ptr to lagrange multiplier box-constraint lower
+ std::vector<double*> lam_lb_;
+ //! ptr to lagrange multiplier box-constraint upper
+ std::vector<double*> lam_ub_;
+ //! ptr to lagrange multiplier general-constraint lower
+ std::vector<double*> lam_lg_;
+ //! ptr to lagrange multiplier general-constraint upper
+ std::vector<double*> lam_ug_;
+
+ //! container lagr. mult. box-constr. lower
+ ct::core::DiscreteArray<Eigen::Matrix<double, max_box_constr_dim, 1>> cont_lam_lb_;
+ //! container lagr. mult. box-constr. upper
+ ct::core::DiscreteArray<Eigen::Matrix<double, max_box_constr_dim, 1>> cont_lam_ub_;
+ //! container for lagr. mult. general-constraint lower
+ ct::core::DiscreteArray<Eigen::Matrix<double, -1, 1>> cont_lam_lg_;
+ //! container for lagr. mult. general-constraint upper
+ ct::core::DiscreteArray<Eigen::Matrix<double, -1, 1>> cont_lam_ug_;
+
+ ct::core::StateVectorArray<STATE_DIM> hx_;
+ ct::core::StateVectorArray<STATE_DIM> hpi_;
+ ct::core::ControlVectorArray<CONTROL_DIM> hu_;
+
+ //! settings from NLOptConSolver
+ NLOptConSettings settings_;
+
+ std::vector<char> qp_mem_;
+ struct d_ocp_qp qp_;
+
+ std::vector<char> qp_sol_mem_;
+ struct d_ocp_qp_sol qp_sol_;
+
+ struct d_ipm_hard_ocp_qp_arg arg_; //! IPM settings
+ std::vector<char> ipm_mem_;
+ struct d_ipm_hard_ocp_qp_workspace workspace_;
+};
+
+
+} // namespace optcon
+} // namespace ct
+
+#endif // HPIPM
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
new file mode 100644
index 0000000..7143b29
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+#include <ct/optcon/problem/LQOCProblem.hpp>
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * Base class for solvers to solve an LQOCProblem
+ * (both constrained / unconstrained, etc.)
+ *
+ * \todo uncouple from NLOptConSettings
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class LQOCSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+
+ /*!
+ * Constructor. Initialize by handing over an LQOCProblem, or otherwise by calling setProblem()
+ * @param lqocProblem shared_ptr to the LQOCProblem to be solved.
+ */
+ LQOCSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr) : lqocProblem_(lqocProblem) {}
+ //! destructor
+ virtual ~LQOCSolver() {}
+ /*!
+ * set a new problem
+ * update the shared_ptr to the LQOCProblem instance and call initialize instance deriving from this class.
+ * @param lqocProblem
+ */
+ void setProblem(std::shared_ptr<LQOCProblem_t> lqocProblem)
+ {
+ setProblemImpl(lqocProblem);
+ lqocProblem_ = lqocProblem;
+ }
+
+
+ virtual void configure(const NLOptConSettings& settings) = 0;
+
+ //! setup and configure the box constraints
+ virtual void configureBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+ {
+ throw std::runtime_error("box constraints are not available for this solver.");
+ }
+
+ //! setup and configure the general (in)equality constraints
+ virtual void configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+ {
+ throw std::runtime_error("general constraints are not available for this solver.");
+ }
+
+ //! a method reserved for memory allocation (e.g. required for HPIPM)
+ virtual void initializeAndAllocate() = 0;
+
+ //! solve the LQOC problem
+ virtual void solve() = 0;
+
+ virtual void solveSingleStage(int N)
+ {
+ throw std::runtime_error("solveSingleStage not available for this solver.");
+ }
+
+ virtual SCALAR getSmallestEigenvalue()
+ {
+ throw std::runtime_error("getSmallestEigenvalue not available for this solver.");
+ }
+
+ virtual void computeStateAndControlUpdates() = 0;
+
+ virtual ct::core::StateVectorArray<STATE_DIM, SCALAR> getSolutionState() = 0;
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getSolutionControl() = 0;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K) = 0;
+
+ const SCALAR& getControlUpdateNorm() { return delta_uff_norm_; }
+ const SCALAR& getStateUpdateNorm() { return delta_x_norm_; }
+ const core::StateVectorArray<STATE_DIM, SCALAR>& getStateUpdates() { return lx_; }
+ const core::ControlVectorArray<CONTROL_DIM, SCALAR>& getControlUpdates() { return lu_; }
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getFeedforwardUpdates() = 0;
+
+protected:
+ virtual void setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem) = 0;
+
+ std::shared_ptr<LQOCProblem_t> lqocProblem_;
+
+ core::StateVectorArray<STATE_DIM, SCALAR> lx_; // differential update on the state
+ core::ControlVectorArray<CONTROL_DIM, SCALAR> lu_; // differential update on the control
+
+ SCALAR delta_uff_norm_; //! l2-norm of the control update
+ SCALAR delta_x_norm_; //! l2-norm of the state update
+};
+} // namespace optcon
+} // namespace ct