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/examples/ConstraintExampleOutput.cpp b/ct_optcon/examples/ConstraintExampleOutput.cpp
new file mode 100644
index 0000000..6d355cb
--- /dev/null
+++ b/ct_optcon/examples/ConstraintExampleOutput.cpp
@@ -0,0 +1,154 @@
+/**********************************************************************************************************************
+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)
+**********************************************************************************************************************/
+
+/*!
+ * This Example displays some constraint toolbox outputs, giving the user a feeling for
+ * sparsity patterns, etc.
+ */
+
+#include <ct/optcon/optcon.h>
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+// some random state and control dimensions
+const size_t state_dim = 10;
+const size_t control_dim = 5;
+
+
+void controlInputBoxConstraintExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired boundaries
+ ControlVector<control_dim> u_lb = -ControlVector<control_dim>::Ones();
+ ControlVector<control_dim> u_ub = ControlVector<control_dim>::Ones();
+
+ // constraint term
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub));
+ controlConstraint->setName("ControlInputConstraint");
+
+ // add and initialize constraint term
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for control input constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void terminalConstraintExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired terminal state
+ StateVector<state_dim> x_final = StateVector<state_dim>::Random();
+
+ // terminal constrain term
+ std::shared_ptr<TerminalConstraint<state_dim, control_dim>> terminalConstraint(
+ new TerminalConstraint<state_dim, control_dim>(x_final));
+ terminalConstraint->setName("TerminalConstraint");
+
+ // add and initialize terminal constraint term
+ constraints->addTerminalConstraint(terminalConstraint, true);
+ constraints->initialize();
+
+ std::cout << "==============================================" << std::endl;
+ std::cout << "Printing example for terminal state constraint:" << std::endl;
+ std::cout << "==============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void boxConstraintsExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired terminal state
+ ControlVector<control_dim> u_lb = -1.11 * ControlVector<control_dim>::Ones();
+ ControlVector<control_dim> u_ub = 1.11 * ControlVector<control_dim>::Ones();
+ StateVector<state_dim> x_lb = -3.33 * StateVector<state_dim>::Ones();
+ StateVector<state_dim> x_ub = 3.33 * StateVector<state_dim>::Ones();
+
+ // constrain terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub));
+ controlConstraint->setName("ControlInputConstraint");
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub));
+ stateConstraint->setName("StateConstraint");
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->addIntermediateConstraint(stateConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for combined box constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void sparseBoxConstraintsExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // box constraint boundaries with sparsities
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 0, 1, 0, 0, 1;
+ Eigen::VectorXd u_lb(2);
+ Eigen::VectorXd u_ub(2);
+ u_lb.setConstant(-1.11);
+ u_ub = -u_lb;
+
+ Eigen::VectorXi sp_state(state_dim);
+ sp_state << 0, 1, 0, 0, 1, 0, 1, 1, 0, 0;
+ Eigen::VectorXd x_lb(4);
+ Eigen::VectorXd x_ub(4);
+ x_lb.setConstant(-3.33);
+ x_ub = -x_lb;
+
+ // constrain terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub, sp_state));
+ stateConstraint->setName("StateConstraint");
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->addIntermediateConstraint(stateConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for sparse box constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+int main(int argc, char **argv)
+{
+ controlInputBoxConstraintExample();
+ terminalConstraintExample();
+ boxConstraintsExample();
+ sparseBoxConstraintsExample();
+ return 1;
+}
diff --git a/ct_optcon/examples/DMS.cpp b/ct_optcon/examples/DMS.cpp
new file mode 100644
index 0000000..37442cf
--- /dev/null
+++ b/ct_optcon/examples/DMS.cpp
@@ -0,0 +1,122 @@
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+/*!
+ * This example shows how to use classical Direct Multiple Shooting with an oscillator system,
+ * using IPOPT as NLP solver.
+ *
+ * \example DMS.cpp
+ */
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon;
+ using namespace ct::core;
+
+ const size_t state_dim = SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = SecondOrderSystem::CONTROL_DIM;
+
+
+ /**
+ * STEP 1 : set up the optimal control problem
+ * */
+
+ StateVector<state_dim> x_0;
+ StateVector<state_dim> x_final;
+
+ x_0 << 0.0, 0.0;
+ x_final << 2.0, -1.0;
+
+ double w_n = 0.5; // oscillator frequency
+ double zeta = 0.01; // oscillator damping
+
+ // create oscillator system
+ std::shared_ptr<SecondOrderSystem> oscillator(new SecondOrderSystem(w_n, zeta));
+
+
+ // load the cost weighting matrices from file and store them in terms. Note that we only use intermediate cost
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/dmsCost.info", "intermediateCost", true);
+
+ // create a cost function and add the terms to it.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+
+
+ // we include the desired terminal state as a hard constraint
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> finalConstraints(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<state_dim, control_dim>> terminalConstraint(
+ new TerminalConstraint<2, 1>(x_final));
+ terminalConstraint->setName("TerminalConstraint");
+ finalConstraints->addTerminalConstraint(terminalConstraint, true);
+ finalConstraints->initialize();
+
+
+ // define optcon problem and add constraint
+ OptConProblem<state_dim, control_dim> optConProblem(oscillator, costFunction);
+ optConProblem.setInitialState(x_0);
+ optConProblem.setGeneralConstraints(finalConstraints);
+
+
+ /**
+ * STEP 2 : determine solver settings
+ */
+ DmsSettings settings;
+ settings.N_ = 25; // number of nodes
+ settings.T_ = 5.0; // final time horizon
+ settings.nThreads_ = 4; // number of threads for multi-threading
+ settings.splineType_ = DmsSettings::PIECEWISE_LINEAR;
+ settings.costEvaluationType_ = DmsSettings::FULL; // we evaluate the full cost and use no trapezoidal approximation
+ settings.objectiveType_ = DmsSettings::KEEP_TIME_AND_GRID; // don't optimize the time spacing between the nodes
+ settings.h_min_ = 0.1; // minimum admissible distance between two nodes in [sec]
+ settings.integrationType_ = DmsSettings::RK4; // type of the shot integrator
+ settings.dt_sim_ = 0.01; // forward simulation dt
+ settings.solverSettings_.solverType_ = NlpSolverSettings::SolverType::IPOPT; // use IPOPT
+ settings.absErrTol_ = 1e-8;
+ settings.relErrTol_ = 1e-8;
+
+ settings.print();
+
+
+ /**
+ * STEP 3 : Calculate an appropriate initial guess
+ */
+ StateVectorArray<state_dim> x_initguess;
+ ControlVectorArray<control_dim> u_initguess;
+ DmsPolicy<state_dim, control_dim> initialPolicy;
+
+
+ x_initguess.resize(settings.N_ + 1, StateVector<state_dim>::Zero());
+ u_initguess.resize(settings.N_ + 1, ControlVector<control_dim>::Zero());
+ for (size_t i = 0; i < settings.N_ + 1; ++i)
+ {
+ x_initguess[i] = x_0 + (x_final - x_0) * (i / settings.N_);
+ }
+
+ initialPolicy.xSolution_ = x_initguess;
+ initialPolicy.uSolution_ = u_initguess;
+
+
+ /**
+ * STEP 4: solve DMS with IPOPT
+ */
+
+ optConProblem.setTimeHorizon(settings.T_);
+
+ std::shared_ptr<DmsSolver<state_dim, control_dim>> dmsSolver(
+ new DmsSolver<state_dim, control_dim>(optConProblem, settings));
+
+ dmsSolver->setInitialGuess(initialPolicy);
+ dmsSolver->solve();
+
+ // retrieve the solution
+ DmsPolicy<state_dim, control_dim> solution = dmsSolver->getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.xSolution_, solution.uSolution_, solution.tSolution_);
+}
diff --git a/ct_optcon/examples/LQR.cpp b/ct_optcon/examples/LQR.cpp
new file mode 100644
index 0000000..dcd2ae0
--- /dev/null
+++ b/ct_optcon/examples/LQR.cpp
@@ -0,0 +1,59 @@
+/*!
+ * Simple example how to linearize a system and design an LQR controller.
+ *
+ * \example LQR.cpp
+ */
+
+
+#include <ct/optcon/optcon.h> // also includes ct_core
+#include "exampleDir.h"
+
+int main(int argc, char** argv)
+{
+ // get the state and control input dimension of the oscillator
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+ // create an auto-differentiable instance of the oscillator dynamics
+ ct::core::ADCGScalar w_n(50.0);
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, ct::core::ADCGScalar>> oscillatorDynamics(
+ new ct::core::tpl::SecondOrderSystem<ct::core::ADCGScalar>(w_n));
+
+ // create an Auto-Differentiation Linearizer with code generation on the quadrotor model
+ ct::core::ADCodegenLinearizer<state_dim, control_dim> adLinearizer(oscillatorDynamics);
+
+ // compile the linearized model just-in-time
+ adLinearizer.compileJIT();
+
+ // define the linearization point around steady state
+ ct::core::StateVector<state_dim> x;
+ x.setZero();
+ ct::core::ControlVector<control_dim> u;
+ u.setZero();
+ double t = 0.0;
+
+ // compute the linearization around the nominal state using the Auto-Diff Linearizer
+ auto A = adLinearizer.getDerivativeState(x, u, t);
+ auto B = adLinearizer.getDerivativeControl(x, u, t);
+
+ // load the weighting matrices
+ ct::optcon::TermQuadratic<state_dim, control_dim> quadraticCost;
+ quadraticCost.loadConfigFile(ct::optcon::exampleDir + "/lqrCost.info", "termLQR");
+ auto Q = quadraticCost.stateSecondDerivative(x, u, t); // x, u and t can be arbitrary here
+ auto R = quadraticCost.controlSecondDerivative(x, u, t); // x, u and t can be arbitrary here
+
+ // design the LQR controller
+ ct::optcon::LQR<state_dim, control_dim> lqrSolver;
+ ct::core::FeedbackMatrix<state_dim, control_dim> K;
+
+ std::cout << "A: " << std::endl << A << std::endl << std::endl;
+ std::cout << "B: " << std::endl << B << std::endl << std::endl;
+ std::cout << "Q: " << std::endl << Q << std::endl << std::endl;
+ std::cout << "R: " << std::endl << R << std::endl << std::endl;
+
+ lqrSolver.compute(Q, R, A, B, K);
+
+ std::cout << "LQR gain matrix:" << std::endl << K << std::endl;
+
+ return 1;
+}
diff --git a/ct_optcon/examples/NLOC.cpp b/ct_optcon/examples/NLOC.cpp
new file mode 100644
index 0000000..73b17f8
--- /dev/null
+++ b/ct_optcon/examples/NLOC.cpp
@@ -0,0 +1,125 @@
+/*!
+ * \example NLOC.cpp
+ *
+ * This example shows how to use the nonlinear optimal control solvers iLQR, unconstrained Gauss-Newton-Multiple-Shooting (GNMS),
+ * as well as the hybrid methods iLQR-GNMS(M), where M denotes the number of multiple-shooting intervals. This example applies
+ * them to a simple second-order oscillator.
+ *
+ */
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+int main(int argc, char **argv)
+{
+ /*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nThreads = 4; // use multi-threading
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver =
+ NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER; // solve LQ-problems using custom Riccati solver
+ ilqr_settings.printSummary = true;
+ ilqr_settings.debugPrint = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/NLOC_MPC.cpp b/ct_optcon/examples/NLOC_MPC.cpp
new file mode 100644
index 0000000..a2fe427
--- /dev/null
+++ b/ct_optcon/examples/NLOC_MPC.cpp
@@ -0,0 +1,164 @@
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+/*!
+ * This tutorial example shows how to use the MPC class. In the CT, every optimal control solver can be wrapped into the MPC-class,
+ * allowing for very rapid prototyping of different MPC applications.
+ * In this example, we apply iLQR-MPC to a simple second order system, a damped oscillator.
+ * This tutorial builds up on the example NLOC.cpp, please consider this one as well.
+ *
+ * \example NLOC_MPC.cpp
+ */
+int main(int argc, char **argv)
+{
+ /* PRELIMINIARIES, see example NLOC.cpp */
+
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/mpcCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/mpcCost.info", "finalCost", verbose);
+
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ ct::core::Time timeHorizon = 3.0;
+
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::
+ GNRICCATI_SOLVER; // the LQ-problems are solved using a custom Gauss-Newton Riccati solver
+ ilqr_settings.printSummary = true;
+
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ ct::core::StateFeedbackController<state_dim, control_dim> initialSolution = iLQR.getSolution();
+
+
+ /* MPC-EXAMPLE
+ * we store the initial solution obtained from solving the initial optimal control problem,
+ * and re-use it to initialize the MPC solver in the following. */
+
+ /* STEP 1: first, we set up an MPC instance for the iLQR solver and configure it. Since the MPC
+ * class is wrapped around normal Optimal Control Solvers, we need to different kind of settings,
+ * those for the optimal control solver, and those specific to MPC: */
+
+ // 1) settings for the iLQR instance used in MPC. Of course, we use the same settings
+ // as for solving the initial problem ...
+ NLOptConSettings ilqr_settings_mpc = ilqr_settings;
+ // ... however, in MPC-mode, it makes sense to limit the overall number of iLQR iterations (real-time iteration scheme)
+ ilqr_settings_mpc.max_iterations = 1;
+ // and we limited the printouts, too.
+ ilqr_settings_mpc.printSummary = false;
+
+
+ // 2) settings specific to model predictive control. For a more detailed description of those, visit ct/optcon/mpc/MpcSettings.h
+ ct::optcon::mpc_settings mpc_settings;
+ mpc_settings.stateForwardIntegration_ = true;
+ mpc_settings.postTruncation_ = true;
+ mpc_settings.measureDelay_ = true;
+ mpc_settings.delayMeasurementMultiplier_ = 1.0;
+ mpc_settings.mpc_mode = ct::optcon::MPC_MODE::FIXED_FINAL_TIME;
+ mpc_settings.coldStart_ = false;
+
+
+ // STEP 2 : Create the iLQR-MPC object, based on the optimal control problem and the selected settings.
+ MPC<NLOptConSolver<state_dim, control_dim>> ilqr_mpc(optConProblem, ilqr_settings_mpc, mpc_settings);
+
+ // initialize it using the previously computed initial controller
+ ilqr_mpc.setInitialGuess(initialSolution);
+
+
+ /* STEP 3: running MPC
+ * Here, we run the MPC loop. Note that the general underlying idea is that you receive a state-estimate
+ * together with a time-stamp from your robot or system. MPC needs to receive both that time information and
+ * the state from your control system. Here, "simulate" the time measurement using std::chrono and wrap
+ * everything into a for-loop.
+ * The basic idea of operation is that after receiving time and state information, one executes the finishIteration() method of MPC.
+ */
+ auto start_time = std::chrono::high_resolution_clock::now();
+
+
+ // limit the maximum number of runs in this example
+ size_t maxNumRuns = 100;
+
+ std::cout << "Starting to run MPC" << std::endl;
+
+ for (size_t i = 0; i < maxNumRuns; i++)
+ {
+ // let's for simplicity, assume that the "measured" state is the first state from the optimal trajectory plus some noise
+ if (i > 0)
+ x0 = 0.1 * StateVector<state_dim>::Random();
+
+ // time which has passed since start of MPC
+ auto current_time = std::chrono::high_resolution_clock::now();
+ ct::core::Time t =
+ 1e-6 * std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count();
+
+ // prepare mpc iteration
+ ilqr_mpc.prepareIteration(t);
+
+ // new optimal policy
+ ct::core::StateFeedbackController<state_dim, control_dim> newPolicy;
+
+ // timestamp of the new optimal policy
+ ct::core::Time ts_newPolicy;
+
+ current_time = std::chrono::high_resolution_clock::now();
+ t = 1e-6 * std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count();
+ bool success = ilqr_mpc.finishIteration(x0, t, newPolicy, ts_newPolicy);
+
+ // we break the loop in case the time horizon is reached or solve() failed
+ if (ilqr_mpc.timeHorizonReached() | !success)
+ break;
+ }
+
+
+ // the summary contains some statistical data about time delays, etc.
+ ilqr_mpc.printMpcSummary();
+}
diff --git a/ct_optcon/examples/NLOC_boxConstrained.cpp b/ct_optcon/examples/NLOC_boxConstrained.cpp
new file mode 100644
index 0000000..1f0d219
--- /dev/null
+++ b/ct_optcon/examples/NLOC_boxConstrained.cpp
@@ -0,0 +1,149 @@
+/*!
+ * \example NLOC_boxConstrained.cpp
+ *
+ * This example shows how to use box constraints alongside NLOC and requires HPIPM to be installed
+ * The unconstrained Riccati backward-pass is replaced by a high-performance interior-point
+ * constrained linear-quadratic Optimal Control solver.
+ *
+ */
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+int main(int argc, char **argv)
+{
+ /*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: set up the box constraints for the control input*/
+ // input box constraint boundaries with sparsities in constraint toolbox format
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 1;
+ Eigen::VectorXd u_lb(control_dim);
+ Eigen::VectorXd u_ub(control_dim);
+ u_lb.setConstant(-0.5);
+ u_ub = -u_lb;
+
+ // constraint terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> boxConstraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // add and initialize constraint terms
+ boxConstraints->addIntermediateConstraint(controlConstraint, verbose);
+ boxConstraints->initialize();
+
+
+ /* STEP 1-E: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+ // add the box constraints to the optimal control problem
+ optConProblem.setBoxConstraints(boxConstraints);
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nThreads = 4;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER; // solve LQ-problems using HPIPM
+ ilqr_settings.lqoc_solver_settings.num_lqoc_iterations = 10; // number of riccati sub-iterations
+ ilqr_settings.printSummary = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/NLOC_generalConstrained.cpp b/ct_optcon/examples/NLOC_generalConstrained.cpp
new file mode 100644
index 0000000..0329fd7
--- /dev/null
+++ b/ct_optcon/examples/NLOC_generalConstrained.cpp
@@ -0,0 +1,198 @@
+/*!
+ * \example NLOC_generalConstrained.cpp
+ *
+ * This example shows how to use general constraints alongside NLOC and requires HPIPM to be installed
+ * The unconstrained Riccati backward-pass is replaced by a high-performance interior-point
+ * constrained linear-quadratic Optimal Control solver.
+ *
+ */
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+/*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+static const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+static const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+/*!
+ * @brief A simple 1d constraint term.
+ *
+ * This term implements the general inequality constraints
+ * \f$ d_{lb} \leq u \cdot p^2 \leq d_{ub} \f$
+ * where \f$ p \f$ denotes the position of the oscillator mass.
+ *
+ * This constraint can be thought of a position-varying bound on the control input.
+ * At large oscillator deflections, the control bounds shrink
+ */
+class ConstraintTerm1D : public ct::optcon::ConstraintBase<state_dim, control_dim>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename ct::core::tpl::TraitSelector<double>::Trait Trait;
+ typedef typename ct::core::tpl::TraitSelector<ct::core::ADCGScalar>::Trait TraitCG;
+ typedef ct::optcon::ConstraintBase<state_dim, control_dim> Base;
+ typedef ct::core::StateVector<state_dim> state_vector_t;
+ typedef ct::core::ControlVector<control_dim> control_vector_t;
+
+ //! constructor with hard-coded constraint boundaries.
+ ConstraintTerm1D()
+ {
+ Base::lb_.resize(1);
+ Base::ub_.resize(1);
+ Base::lb_.setConstant(-0.5);
+ Base::ub_.setConstant(0.5);
+ }
+
+ virtual ~ConstraintTerm1D() {}
+ virtual ConstraintTerm1D* clone() const override { return new ConstraintTerm1D(); }
+ virtual size_t getConstraintSize() const override { return 1; }
+ virtual Eigen::VectorXd evaluate(const state_vector_t& x, const control_vector_t& u, const double t) override
+ {
+ Eigen::Matrix<double, 1, 1> val;
+ val.template segment<1>(0) << u(0) * x(0) * x(0);
+ return val;
+ }
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const ct::core::StateVector<state_dim, ct::core::ADCGScalar>& x,
+ const ct::core::ControlVector<control_dim, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ Eigen::Matrix<ct::core::ADCGScalar, 1, 1> val;
+
+ val.template segment<1>(0) << u(0) * x(0) * x(0);
+
+ return val;
+ }
+};
+
+
+int main(int argc, char** argv)
+{
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: set up the general constraints */
+ // constraint terms
+ std::shared_ptr<ConstraintTerm1D> pathConstraintTerm(new ConstraintTerm1D());
+
+ // create constraint container
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> generalConstraints(
+ new ct::optcon::ConstraintContainerAD<state_dim, control_dim>());
+
+
+ // add and initialize constraint terms
+ generalConstraints->addIntermediateConstraint(pathConstraintTerm, verbose);
+ generalConstraints->initialize();
+
+
+ /* STEP 1-E: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+ // add the box constraints to the optimal control problem
+ optConProblem.setGeneralConstraints(generalConstraints);
+
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.min_cost_improvement = 1e-6;
+ ilqr_settings.meritFunctionRhoConstraints = 10;
+ ilqr_settings.nThreads = 1;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER; // solve LQ-problems using HPIPM
+ ilqr_settings.lqoc_solver_settings.num_lqoc_iterations = 10; // number of riccati sub-iterations
+ ilqr_settings.lineSearchSettings.active = true;
+ ilqr_settings.lineSearchSettings.debugPrint = true;
+ ilqr_settings.printSummary = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/dmsCost.info b/ct_optcon/examples/dmsCost.info
new file mode 100644
index 0000000..59a6a0e
--- /dev/null
+++ b/ct_optcon/examples/dmsCost.info
@@ -0,0 +1,26 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 1
+ }
+ R
+ {
+ scaling 0.1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/exampleDir.h.in b/ct_optcon/examples/exampleDir.h.in
new file mode 100644
index 0000000..3d6b34c
--- /dev/null
+++ b/ct_optcon/examples/exampleDir.h.in
@@ -0,0 +1,15 @@
+/**********************************************************************************************************************
+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 {
+
+static const std::string exampleDir = "${CT_OPTCON_EXAMPLE_DIR}";
+
+}
+}
diff --git a/ct_optcon/examples/lqrCost.info b/ct_optcon/examples/lqrCost.info
new file mode 100644
index 0000000..18f9cf3
--- /dev/null
+++ b/ct_optcon/examples/lqrCost.info
@@ -0,0 +1,21 @@
+termLQR
+{
+ name "state cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 1.6
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 1.0
+ (0,0) 1.0
+ }
+ }
+}
diff --git a/ct_optcon/examples/mpcCost.info b/ct_optcon/examples/mpcCost.info
new file mode 100644
index 0000000..46f9244
--- /dev/null
+++ b/ct_optcon/examples/mpcCost.info
@@ -0,0 +1,54 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 0.1
+ }
+ R
+ {
+ scaling 1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
+
+
+finalCost
+{
+ name "final cost quadratic"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 10.0
+ (0,0) 1.0
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 0
+ (0,0) 0
+ }
+ x_des
+ {
+ (0,0) 2
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/nlocCost.info b/ct_optcon/examples/nlocCost.info
new file mode 100644
index 0000000..f68ae38
--- /dev/null
+++ b/ct_optcon/examples/nlocCost.info
@@ -0,0 +1,54 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 0.1
+ }
+ R
+ {
+ scaling 1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
+
+
+finalCost
+{
+ name "final cost quadratic"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 100.0
+ (0,0) 1.0
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 0
+ (0,0) 0
+ }
+ x_des
+ {
+ (0,0) 1.5
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/plotResultsOscillator.h b/ct_optcon/examples/plotResultsOscillator.h
new file mode 100644
index 0000000..08af55a
--- /dev/null
+++ b/ct_optcon/examples/plotResultsOscillator.h
@@ -0,0 +1,68 @@
+/**********************************************************************************************************************
+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
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+void plotResultsOscillator(const ct::core::StateVectorArray<STATE_DIM>& stateArray,
+ const ct::core::ControlVectorArray<CONTROL_DIM>& controlArray,
+ const ct::core::TimeArray& timeArray)
+{
+#ifdef PLOTTING_ENABLED
+
+ using namespace ct::core;
+
+ try
+ {
+ plot::ion();
+ plot::figure();
+
+ if (timeArray.size() != stateArray.size())
+ {
+ std::cout << timeArray.size() << std::endl;
+ std::cout << stateArray.size() << std::endl;
+ throw std::runtime_error("Cannot plot data, x and t not equal length");
+ }
+
+ std::vector<double> position;
+ std::vector<double> velocity;
+ std::vector<double> time_state;
+ for (size_t j = 0; j < stateArray.size(); j++)
+ {
+ position.push_back(stateArray[j](0));
+ velocity.push_back(stateArray[j](1));
+ time_state.push_back(timeArray[j]);
+ }
+
+ std::vector<double> control;
+ std::vector<double> time_control;
+ for (size_t j = 0; j < controlArray.size(); j++)
+ {
+ control.push_back(controlArray[j](0));
+ time_control.push_back(timeArray[j]);
+ }
+
+ plot::subplot(3, 1, 1);
+ plot::plot(time_state, position);
+ plot::title("position");
+
+ plot::subplot(3, 1, 2);
+ plot::plot(time_state, velocity);
+ plot::title("velocity");
+
+ plot::subplot(3, 1, 3);
+ plot::plot(time_control, control);
+ plot::title("control");
+
+ plot::show();
+ } catch (const std::exception& e)
+ {
+ std::cout << e.what() << std::endl;
+ }
+#else
+ std::cout << "Plotting is disabled." << std::endl;
+#endif
+}