From 6ee68c5978b02557a6b5aed8bc4284495dd038ce Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Fri, 24 Apr 2020 18:31:15 +0200
Subject: [PATCH 01/21] backing up the test

---
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 229 ++++++++++++++++++
 1 file changed, 229 insertions(+)
 create mode 100644 ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp

diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
new file mode 100644
index 000000000..01918bc24
--- /dev/null
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -0,0 +1,229 @@
+
+#include <ct/optcon/optcon.h>
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+const bool verbose = true;
+
+using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
+const size_t state_dim = ManifoldState_t::TangentDim;
+const size_t control_dim = 3;
+
+class DiscrSO3LTITestSystem final : public LTISystem<ManifoldState_t, control_dim, DISCRETE_TIME>
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    DiscrSO3LTITestSystem(const Eigen::Matrix<double, state_dim, state_dim> A_test,
+        const Eigen::Matrix<double, control_dim, control_dim> B_test,
+        const ManifoldState_t& m_ref = ManifoldState_t::NeutralElement())
+        : m_ref_(m_ref)
+    {
+        this->A() << A_test;
+        this->B() << B_test;
+    }
+
+    /**
+     * @brief the log operator is defined as expressing the tangent vector w.r.t. m_ref_
+     */
+    virtual ManifoldState_t::Tangent lift(const ManifoldState_t& m) override
+    {
+        return ManifoldState_t::Tangent::Zero();  // the system stays where it is (combination with A = identity())
+        /*return m.rminus(m);*/
+    }
+    // virtual ManifoldState_t retract(const ManifoldState_t::Tangent& t) override {
+    //     throw std::runtime_error("not implemetned.");
+    //     /*return m_ref_.rplus(t);*/ }
+protected:
+    ManifoldState_t m_ref_;
+};
+
+
+int main(int argc, char** argv)
+{
+    std::cout << std::fixed;
+
+    const size_t N = 5;
+    const double dt = 0.25;
+
+    const ManifoldState_t x0 = manif::SO3<double>(0, 0, 0);
+    ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
+    ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj_init(N);  // init control traj
+    for (size_t i = 0; i < N; i++)
+        u_traj_init[i] = ct::core::ControlVector<control_dim>::Zero() * dt;
+
+
+    // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
+        new HPIPMInterface<ManifoldState_t, control_dim>);
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
+        new GNRiccatiSolver<ManifoldState_t, control_dim>);
+
+    // store them, and identifying names, in a vectors
+    std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
+    lqocSolvers.push_back(gnRiccatiSolver);
+    lqocSolvers.push_back(hpipmSolver);
+    std::vector<std::string> solverNames = {"Riccati", "HPIPM"};
+
+    // create linear-quadratic optimal control problem containers
+    std::vector<std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>>> problems;
+    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem1(
+        new LQOCProblem<ManifoldState_t, control_dim>(N));
+    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem2(
+        new LQOCProblem<ManifoldState_t, control_dim>(N));
+
+    problems.push_back(lqocProblem1);
+    problems.push_back(lqocProblem2);
+
+    // create a discrete-time manifold system
+    Eigen::Matrix<double, state_dim, state_dim> A = Eigen::Matrix<double, state_dim, state_dim>::Zero();
+    Eigen::Matrix<double, control_dim, control_dim> B;
+    A << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0;  // a slightly unstable system
+    B << 1, 0, 0, 0, 1, 0, 0, 0, 1;        // direct action
+    std::shared_ptr<LinearSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
+        new DiscrSO3LTITestSystem(A, B, x0));
+
+
+    // create a cost function
+    Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
+    Eigen::Matrix<double, control_dim, control_dim> R;
+    Q_final << 100, 0, 0, 0, 100, 0, 0, 0, 100;
+    //Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    Q.setZero();
+    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    ManifoldState_t x_final = manif::SO3<double>(M_PI / 2, 0, 0);
+    std::cout << "desired final state: " << x_final << std::endl;
+    ManifoldState_t x_nominal = x0;
+    ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
+    std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
+        new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
+
+    ManifoldState_t::Tangent b;
+    b.setZero();  // todo why this?
+
+    // integrate an initial state with the open-loop system to get initial trajectories
+    ManifoldState_t x_curr;
+    ManifoldState_t::Tangent dx;
+    x_curr = x0;
+    x_traj.front() = x0;
+    std::cout << "integrate an random initial state with the unstable system" << std::endl;
+    std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    for (size_t i = 0; i < N; i++)
+    {
+        exampleSystem->computeControlledDynamics(x_curr, 0, u_traj_init[i], dx);
+        x_curr = x_curr + dx;
+        x_traj[i + 1] = x_curr;
+        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    }
+
+
+    // initialize the optimal control problems for both solvers
+    problems[0]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj_init, *exampleSystem, *costFunction, b, dt);
+    problems[1]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj_init, *exampleSystem, *costFunction, b, dt);
+
+
+    // HACKY corrections // TODO: move somewhere meaningful
+    for (size_t idx : {0, 1})  // for both solvers
+    {
+        // intermediate stages cost transportation
+        for (size_t i = 0; i < N; i++)
+        {
+            Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
+            auto e = x_nominal.rminus(x_traj[i], Jl, Jr);  // compute PT matrix w.r.t. current ref traj
+            auto e_adj = (e.exp()).adj();
+            problems[idx]->Q_[i] = e_adj.transpose() * problems[idx]->Q_[i] * e_adj;
+            problems[idx]->qv_[i] = e_adj.transpose() * problems[idx]->qv_[i];
+        }
+        // terminal stage transportation
+        Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
+        auto e = x_final.rminus(x_traj[N], Jl, Jr);  // compute PT matrix w.r.t. current ref traj
+        auto e_adj = (e.exp()).adj();
+        std::cout << "cost adj" << std::endl << e_adj << std::endl;
+        std::cout << "cost Jr" << std::endl << Jr << std::endl;
+        problems[idx]->Q_.back() = e_adj.transpose() * problems[idx]->Q_.back() * e_adj;
+        problems[idx]->qv_.back() = e_adj.transpose() * problems[idx]->qv_.back();
+
+        // dynamics transportation
+        for (size_t i = 0; i < N; i++)
+        {
+            // std::cout << "dyn transport matrices" << std::endl;
+            Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
+
+            auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);  // TODO: is it the right way round?
+            auto l_adj = (l.exp()).adj();
+            //std::cout << "Jl" << std::endl << Jl << std::endl;
+            //std::cout << "Jr" << std::endl << Jr << std::endl;
+            //std::cout << "l_adj" << std::endl << l_adj << std::endl;
+            //std::cout << std::endl;
+            problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];  // todo: are those the right
+            problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];  // TODO: are those the right
+        }
+
+
+        // set the problem pointers
+        lqocSolvers[idx]->setProblem(problems[idx]);
+
+        // allocate memory (if required)
+        lqocSolvers[idx]->initializeAndAllocate();
+
+        // solve the problems...
+        lqocSolvers[idx]->solve();
+
+        // postprocess data
+        lqocSolvers[idx]->computeStatesAndControls();
+        lqocSolvers[idx]->computeFeedbackMatrices();
+        lqocSolvers[idx]->compute_lv();
+    }
+
+    // retrieve solutions from both solvers
+    auto xSol_riccati = lqocSolvers[0]->getSolutionState();
+    auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
+    ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
+    ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
+    auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
+    auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
+    ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = lqocSolvers[1]->getSolutionFeedback();
+    ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = lqocSolvers[1]->get_lv();
+
+    std::cout << std::endl << std::endl;
+    std::cout << std::setprecision(3) << "Forward integrated closed-loop solution:" << std::endl;
+    x_curr = x0;
+    std::cout << "m init: " << x_curr << std::endl;
+    for (size_t i = 0; i < N; i++)
+    {
+        Eigen::Matrix<double, state_dim, state_dim> j1, j2;
+        auto x_err = x_curr.rminus(x_traj[i], j1, j2);
+        // std::cout << "lv_sol_riccati[i]: " << lv_sol_riccati[i].transpose() << std::endl;
+        // std::cout << "uSol_riccati[i]: " << uSol_riccati[i].transpose() << std::endl;
+        // std::cout << "KSol_riccati: " << std::endl << KSol_riccati[i] << std::endl;
+        // TODO: some term is missing here;
+        auto u = u_traj_init[i] + lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
+        exampleSystem->computeControlledDynamics(x_curr, i * dt, u, dx);
+        Eigen::Quaterniond old_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
+        x_curr = x_curr + dx;
+        Eigen::Quaterniond new_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
+        std::cout << "m: " << x_curr << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
+    }
+    std::cout << std::endl << std::endl;
+
+    std::cout << std::setprecision(3) << "dx solution from riccati solver and directly added state_traj" << std::endl;
+    ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
+    for (size_t j = 0; j < xSol_riccati.size(); j++)
+    {
+        x_solver_direct[j] = x_traj[j] + xSol_riccati[j];
+        double angularDiff = 0;
+        if (j > 0)
+        {
+            Eigen::Quaterniond old_rot(x_solver_direct[j - 1].w(), x_solver_direct[j - 1].x(),
+                x_solver_direct[j - 1].y(), x_solver_direct[j - 1].z());
+            Eigen::Quaterniond new_rot(
+                x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
+            angularDiff = old_rot.angularDistance(new_rot);
+        }
+        std::cout << "\t m:" << x_solver_direct[j] << "\t dx:" << xSol_riccati[j].transpose()
+                  << "\t -- rot diff norm(): " << angularDiff << std::endl;
+    }
+    return 1;
+}
\ No newline at end of file

From 55d2d87e11a2bc18348450b98348d4ccf8c6cb2b Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Sun, 26 Apr 2020 11:12:03 +0200
Subject: [PATCH 02/21] backing up working example

---
 .../control/StateFeedbackController-impl.h    |  79 ++--
 .../ct/core/control/StateFeedbackController.h |  46 +-
 .../optcon/costfunction/CostFunction-impl.hpp |   2 +-
 .../costfunction/CostFunctionAD-impl.hpp      |   6 +-
 .../ct/optcon/costfunction/CostFunctionAD.hpp |   6 +-
 .../CostFunctionAnalytical-impl.hpp           |   6 +-
 .../costfunction/CostFunctionAnalytical.hpp   |   4 +-
 .../CostFunctionQuadratic-impl.hpp            |  11 +-
 .../costfunction/CostFunctionQuadratic.hpp    |  18 +-
 .../CostFunctionQuadraticSimple-impl.hpp      | 103 +++--
 .../CostFunctionQuadraticSimple.hpp           |  37 +-
 .../ct/optcon/costfunction/costfun-impl.hpp   |   2 +-
 .../ct/optcon/costfunction/costfun.hpp        |   2 +-
 .../ct/optcon/nloc/NLOCBackendBase-impl.hpp   | 405 ++++++++----------
 .../ct/optcon/nloc/NLOCBackendBase.hpp        |  86 ++--
 ct_optcon/include/ct/optcon/optcon.h          |  12 +-
 .../ct/optcon/problem/LQOCProblem-impl.hpp    | 131 ++++--
 .../include/ct/optcon/problem/LQOCProblem.hpp |  27 +-
 .../solver/lqp/AugGNRiccatiSolver-impl.hpp    | 293 +++++++++++++
 .../optcon/solver/lqp/AugGNRiccatiSolver.hpp  | 111 +++++
 .../solver/lqp/GNRiccatiSolver-impl.hpp       |  74 ++--
 .../ct/optcon/solver/lqp/GNRiccatiSolver.hpp  |  13 +-
 .../optcon/solver/lqp/HPIPMInterface-impl.hpp |  87 ++--
 .../ct/optcon/solver/lqp/HPIPMInterface.hpp   |  32 +-
 .../ct/optcon/solver/lqp/LQOCSolver.hpp       |  21 +-
 ct_optcon/test/CMakeLists.txt                 |  11 +-
 .../test/solver/linear/HPIPMInterfaceTest.h   |  18 +-
 ct_optcon/test/solver/linear/LQOCSolverTest.h |  27 +-
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 293 +++++++------
 ct_optcon/test/testSystems/LinkedMasses.h     |  15 +-
 ct_optcon/test/testSystems/SpringLoadedMass.h |  36 +-
 31 files changed, 1223 insertions(+), 791 deletions(-)
 create mode 100644 ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
 create mode 100644 ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp

diff --git a/ct_core/include/ct/core/control/StateFeedbackController-impl.h b/ct_core/include/ct/core/control/StateFeedbackController-impl.h
index 397527405..fcb6e61ee 100644
--- a/ct_core/include/ct/core/control/StateFeedbackController-impl.h
+++ b/ct_core/include/ct/core/control/StateFeedbackController-impl.h
@@ -6,17 +6,17 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 namespace ct {
 namespace core {
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController() : Base()
 {
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::~StateFeedbackController()
 {
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(const MANIFOLD& x_ref,
     const ct::core::ControlVector<CONTROL_DIM, SCALAR>& uff,
     const ct::core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>& K)
@@ -29,16 +29,15 @@ StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
 {
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
     const StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>& other)
     : Base(), x_ref_(other.x_ref_), uff_(other.uff_), K_(other.K_)
 {
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
-    const StateVectorArray<STATE_DIM, SCALAR>& x_ref,
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(const DiscreteArray<MANIFOLD>& x_ref,
     const ControlVectorArray<CONTROL_DIM, SCALAR>& uff,
     const FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K,
     const SCALAR deltaT,
@@ -48,8 +47,8 @@ StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::StateFeedbackController(
 {
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const DiscreteArray<state_vector_t>& x_ref,
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const DiscreteArray<MANIFOLD>& x_ref,
     const DiscreteArray<control_vector_t>& uff,
     const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>& K,
     const tpl::TimeArray<SCALAR>& t)
@@ -70,7 +69,7 @@ void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::update(const Discre
     K_.setTime(tshort);
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl(const MANIFOLD& state,
     const Time_t& tn,
     control_vector_t& u)
@@ -78,87 +77,83 @@ void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl(cons
     computeControl_specialized(state, tn, u);
 }
 
-
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>* StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::clone()
     const
 {
     return new StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>(*this);
 }
 
-
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const DiscreteArray<typename StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::state_vector_t>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::x_ref() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+const DiscreteArray<MANIFOLD>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::x_ref() const
 {
     return x_ref_.getDataArray();
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 const DiscreteArray<typename StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::control_vector_t>&
 StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::uff() const
 {
     return uff_.getDataArray();
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::K() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::K() const
+    -> const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>&
 {
     return K_.getDataArray();
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const tpl::TimeArray<SCALAR>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::time() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::time() const -> const tpl::TimeArray<SCALAR>&
 {
     return x_ref_.getTimeArray();
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-StateTrajectory<STATE_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory()
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+DiscreteTrajectory<MANIFOLD>& StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory()
 {
     return x_ref_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const StateTrajectory<STATE_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getReferenceStateTrajectory() const
+    -> const DiscreteTrajectory<MANIFOLD>&
 {
     return x_ref_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-ControlTrajectory<CONTROL_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory()
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory()
+    -> ControlTrajectory<CONTROL_DIM, SCALAR>&
 {
     return uff_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const ControlTrajectory<CONTROL_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedforwardTrajectory() const
+    -> const ControlTrajectory<CONTROL_DIM, SCALAR>&
 {
     return uff_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory()
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory()
+    -> FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
 {
     return K_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
-const FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory() const
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
+auto StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::getFeedbackTrajectory() const
+    -> const FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>&
 {
     return K_;
 }
 
-template <typename MANIFOLD, size_t CONTROL_DIM, typename TIME_T>
+template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
 void StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::extractControlTrajectory(
-    const StateTrajectory<STATE_DIM, SCALAR>& x_traj,
+    const DiscreteTrajectory<MANIFOLD>& x_traj,
     ControlTrajectory<CONTROL_DIM, SCALAR>& u_traj)
 {
     u_traj.clear();
diff --git a/ct_core/include/ct/core/control/StateFeedbackController.h b/ct_core/include/ct/core/control/StateFeedbackController.h
index a93c978dc..3478e3148 100644
--- a/ct_core/include/ct/core/control/StateFeedbackController.h
+++ b/ct_core/include/ct/core/control/StateFeedbackController.h
@@ -6,6 +6,7 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 #pragma once
 
 #include "Controller.h"
+#include <ct/core/systems/System.h>
 
 namespace ct {
 namespace core {
@@ -37,8 +38,8 @@ namespace core {
  * @tparam CONTROL_DIM control vector size
  * @tparam SCALAR primitive type
  */
-template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
-class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -87,6 +88,22 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
     //! deep cloning
     StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>* clone() const override;
 
+    template <ct::core::TIME_TYPE T = TIME_T>
+    typename std::enable_if<T == ct::core::CONTINUOUS_TIME>::type computeControl_specialized(const MANIFOLD& state,
+        const Time_t& t,
+        control_vector_t& u)
+    {
+        u = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t));  // TODO: move to impl
+    }
+
+    template <ct::core::TIME_TYPE T = TIME_T>
+    typename std::enable_if<T == ct::core::DISCRETE_TIME>::type computeControl_specialized(const MANIFOLD& state,
+        const Time_t& n,
+        control_vector_t& u)
+    {
+        u = uff_[n] + K_[n] * (state - x_ref_[n]);  // TODO: move to impl
+    }
+
     //! computes the control action in the continuous case
     /*!
      * evaluates the controller using interpolation where required using interpolation
@@ -96,13 +113,6 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
      */
     virtual void computeControl(const MANIFOLD& state, const Time_t& tn, control_vector_t& controlAction) override;
 
-    typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type computeControl_specialized(const MANIFOLD& state,
-        const Time_t& tn,
-        control_vector_t& controlAction);
-    typename std::enable_if<TIME_T == ct::core::DISCRETE_TIME>::type computeControl_specialized(const MANIFOLD& state,
-        const Time_t& tn,
-        control_vector_t& controlAction);
-
     //! updates the controller
     /*!
      * sets a new feedforward and feedback controller
@@ -156,23 +166,5 @@ class StateFeedbackController : public Controller<MANIFOLD, CONTROL_DIM, TIME_T>
 };
 
 
-template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
-typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl_specialized(const state_vector_t& state,
-    const SCALAR& t,
-    control_vector_t& controlAction)
-{
-    controlAction = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t));
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM, TIME_TYPE TIME_T>
-typename std::enable_if<TIME_T == ct::core::CONTINUOUS_TIME>::type
-StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T>::computeControl_specialized(const state_vector_t& state,
-    const int n,
-    control_vector_t& controlAction)
-{
-    controlAction = uff_[n] + K_[n] * (state - x_ref_[n]);
-}
-
 }  // namespace core
 }  // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
index 3a3807460..28454e1d6 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
@@ -12,7 +12,7 @@ template <typename MANIFOLD, size_t CONTROL_DIM>
 CostFunction<MANIFOLD, CONTROL_DIM>::CostFunction() : t_(0.0), 
 t_shift_(0.0)
 {
-    x_.setZero();
+    x_ = MANIFOLD::NeutralElement();
     u_.setZero();
 }
 
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
index 447acd332..e9b6e21b0 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
@@ -219,8 +219,7 @@ auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR_EVAL
 }
 
 template <typename AD_MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
-    -> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate() -> typename EVAL_MANIFOLD::Tangent
 {
     Eigen::Matrix<SCALAR_EVAL, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
         intermediateCostCodegen_->jacobian(stateControlTime_);
@@ -228,8 +227,7 @@ auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
 }
 
 template <typename AD_MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
-    -> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+auto CostFunctionAD<AD_MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal() -> typename EVAL_MANIFOLD::Tangent
 {
     Eigen::Matrix<SCALAR_EVAL, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
     return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeTerminalBase();
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
index 5c8beccb2..c77c54240 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
@@ -89,13 +89,11 @@ class CostFunctionAD
 	 */
     CostFunctionAD(const CostFunctionAD& arg);
 
-
     /**
 	 * \brief Destructor
 	 */
     virtual ~CostFunctionAD();
 
-
     /**
 	 * @brief      Initializes the AD costfunction, generates and compiles
 	 *             source code
@@ -133,8 +131,8 @@ class CostFunctionAD
     SCALAR_EVAL evaluateIntermediate() override;
     SCALAR_EVAL evaluateTerminal() override;
 
-    ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeIntermediate() override;
-    ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeTerminal() override;
+    typename EVAL_MANIFOLD::Tangent stateDerivativeIntermediate() override;
+    typename EVAL_MANIFOLD::Tangent stateDerivativeTerminal() override;
 
     control_vector_t controlDerivativeIntermediate() override;
     control_vector_t controlDerivativeTerminal() override;
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
index 1138c48ae..a92462332 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
@@ -101,15 +101,13 @@ auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
-    -> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate() -> typename MANIFOLD::Tangent
 {
     return this->stateDerivativeIntermediateBase();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
-    -> ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+auto CostFunctionAnalytical<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal() -> typename MANIFOLD::Tangent
 {
     return this->stateDerivativeTerminalBase();
 }
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
index e90622153..901441d5a 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
@@ -75,8 +75,8 @@ class CostFunctionAnalytical : public CostFunctionQuadratic<MANIFOLD, CONTROL_DI
     SCALAR_EVAL evaluateIntermediate() override;
     SCALAR_EVAL evaluateTerminal() override;
 
-    ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeIntermediate() override;
-    ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeTerminal() override;
+    typename MANIFOLD::Tangent stateDerivativeIntermediate() override;
+    typename MANIFOLD::Tangent stateDerivativeTerminal() override;
 
     state_matrix_t stateSecondDerivativeIntermediate() override;
     state_matrix_t stateSecondDerivativeTerminal() override;
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
index ad35a9505..13f452307 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
@@ -207,7 +207,6 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateIntermediateBase() ->
     return y;
 }
 
-
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateTerminalBase() -> SCALAR_EVAL
 {
@@ -219,11 +218,10 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::evaluateTerminalBase() -> SCA
     return y;
 }
 
-
 template <typename MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBase() -> Tangent_t
+auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBase() -> typename EVAL_MANIFOLD::Tangent
 {
-    Tangent_t derivative;
+    typename EVAL_MANIFOLD::Tangent derivative;
     derivative.setZero();
 
     for (auto it : this->intermediateCostAnalytical_)
@@ -238,11 +236,10 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateBa
     return derivative;
 }
 
-
 template <typename MANIFOLD, size_t CONTROL_DIM>
-auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminalBase() -> Tangent_t
+auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminalBase() -> typename EVAL_MANIFOLD::Tangent
 {
-    Tangent_t derivative;
+    typename EVAL_MANIFOLD::Tangent derivative;
     derivative.setZero();
 
     for (auto it : this->finalCostAnalytical_)
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
index c41bd9fbb..326af56d9 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
@@ -33,7 +33,7 @@ class CostFunctionQuadratic : public CostFunction<MANIFOLD, CONTROL_DIM>
 
     static constexpr size_t STATE_DIM = MANIFOLD::TangentDim;
     using SCALAR_EVAL = typename MANIFOLD::Scalar;  // TODO: is there a better name than scalar_eval?
-    using Tangent_t = typename MANIFOLD::Tangent;
+    using EVAL_MANIFOLD = typename MANIFOLD::template RedefineScalar<typename ct::core::get_out_type<MANIFOLD>::type>;
 
     using Base = CostFunction<MANIFOLD, CONTROL_DIM>;
     using control_vector_t = typename Base::control_vector_t;
@@ -91,13 +91,13 @@ class CostFunctionQuadratic : public CostFunction<MANIFOLD, CONTROL_DIM>
 	 * \brief Computes intermediate-cost first-order derivative with respect to state
 	 * @return derivative vector (Jacobian)
 	 */
-    virtual ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeIntermediate() = 0;
+    virtual typename MANIFOLD::Tangent stateDerivativeIntermediate() = 0;
 
     /**
 	 * Computes terminal-cost first-order derivative with respect to state
 	 * @return derivative vector (Jacobian)
 	 */
-    virtual ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeTerminal() = 0;
+    virtual typename MANIFOLD::Tangent stateDerivativeTerminal() = 0;
 
     /**
 	 * \brief Computes intermediate-cost second-order derivative with respect to state
@@ -184,10 +184,10 @@ class CostFunctionQuadratic : public CostFunction<MANIFOLD, CONTROL_DIM>
     SCALAR_EVAL evaluateTerminalBase();
 
     //! evaluate intermediate analytical state derivatives
-    Tangent_t stateDerivativeIntermediateBase();
+    typename EVAL_MANIFOLD::Tangent stateDerivativeIntermediateBase();
 
     //! evaluate terminal analytical state derivatives
-    Tangent_t stateDerivativeTerminalBase();
+    typename EVAL_MANIFOLD::Tangent stateDerivativeTerminalBase();
 
     //! evaluate intermediate analytical state second derivatives
     state_matrix_t stateSecondDerivativeIntermediateBase();
@@ -217,10 +217,10 @@ class CostFunctionQuadratic : public CostFunction<MANIFOLD, CONTROL_DIM>
     //! compute the state derivative by numerical differentiation (can be used for testing)
     // euclidean case
     template <typename M = MANIFOLD, typename std::enable_if<ct::core::is_euclidean<M>::value, bool>::type>
-    Tangent_t stateDerivativeIntermediateNumDiff();
+    typename EVAL_MANIFOLD::Tangent stateDerivativeIntermediateNumDiff();
     // non-euclidean case
     template <typename M = MANIFOLD, typename std::enable_if<!(ct::core::is_euclidean<M>::value), bool>::type>
-    Tangent_t stateDerivativeIntermediateNumDiff();
+    typename EVAL_MANIFOLD::Tangent stateDerivativeIntermediateNumDiff();
 
     //! compute the control derivative by numerical differentiation (can be used for testing)
     // euclidean case
@@ -247,7 +247,7 @@ class CostFunctionQuadratic : public CostFunction<MANIFOLD, CONTROL_DIM>
 // impl for euclidean case (do not move to *-impl.h file)
 template <typename MANIFOLD, size_t CONTROL_DIM>
 template <typename M, typename std::enable_if<ct::core::is_euclidean<M>::value, bool>::type>
-auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateNumDiff() -> Tangent_t
+auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateNumDiff() -> typename EVAL_MANIFOLD::Tangent
 {
     using state_vector_t = ct::core::StateVector<STATE_DIM, SCALAR_EVAL>;
 
@@ -297,7 +297,7 @@ auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateNu
 // impl for manifold case (do not move to *-impl.h file)
 template <typename MANIFOLD, size_t CONTROL_DIM>
 template <typename M, typename std::enable_if<!(ct::core::is_euclidean<M>::value), bool>::type>
-auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateNumDiff() -> Tangent_t
+auto CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediateNumDiff() -> typename EVAL_MANIFOLD::Tangent
 {
     throw std::runtime_error("stateDerivativeIntermediateNumDiff not implemented for manifold case.");
 }
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index d85f5d016..133868cab 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -8,8 +8,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 namespace ct {
 namespace optcon {
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple()
 {
     x_nominal_.setZero();
     Q_.setZero();
@@ -21,12 +21,12 @@ CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadrat
     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,
+template <typename MANIFOLD, size_t CONTROL_DIM>
+CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(const state_matrix_t& Q,
     const control_matrix_t& R,
-    const state_vector_t& x_nominal,
+    const MANIFOLD& x_nominal,
     const control_vector_t& u_nominal,
-    const state_vector_t& x_final,
+    const MANIFOLD& 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)
 {
@@ -34,14 +34,13 @@ CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadrat
     u_deviation_.setZero();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionQuadraticSimple()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::~CostFunctionQuadraticSimple()
 {
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple(
-    const CostFunctionQuadraticSimple& arg)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(const CostFunctionQuadraticSimple& arg)
     : x_deviation_(arg.x_deviation_),
       x_nominal_(arg.x_nominal_),
       Q_(arg.Q_),
@@ -53,16 +52,14 @@ CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadrat
 {
 }
 
-
-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
+template <typename MANIFOLD, size_t CONTROL_DIM>
+CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>* CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::clone() const
 {
-    return new CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+    return new CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>(*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,
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::setCurrentStateAndControl(const MANIFOLD& x,
     const control_vector_t& u,
     const SCALAR& t)
 {
@@ -70,87 +67,85 @@ void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStat
     this->u_ = u;
     this->t_ = t;
 
-    this->x_deviation_ = x - x_nominal_;
+    this->x_deviation_ = x - x_nominal_;  // TODO: switch order and evluate jacs as well
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate() -> SCALAR
 {
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeIntermediate() -> state_matrix_t
 {
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::controlDerivativeIntermediate() -> control_vector_t
 {
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::controlSecondDerivativeIntermediate() -> control_matrix_t
 {
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateControlDerivativeIntermediate() -> control_state_matrix_t
 {
     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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
 {
-    state_vector_t x_deviation_final = this->x_ - x_final_;
-    return SCALAR(0.5) * x_deviation_final.transpose() * Q_final_ * x_deviation_final;
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
+    return SCALAR(0.5) * (x_deviation_final.transpose() * Q_final_ * x_deviation_final)(0);
 }
 
-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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
 {
-    state_vector_t x_deviation_final = this->x_ - x_final_;
-    return Q_final_ * x_deviation_final;
+    Eigen::Matrix<double, STATE_DIM, STATE_DIM> J_curr, J_ref;
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
+    return Q_final_ * J_curr.transpose() * x_deviation_final;
+    //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
 }
 
-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()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
-    return Q_final_;
+    Eigen::Matrix<double, STATE_DIM, STATE_DIM> J_curr, J_ref;
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
+    return J_curr * Q_final_ * J_curr.transpose();
+    //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
+    //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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::updateReferenceState(const MANIFOLD& 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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::updateFinalState(const MANIFOLD& 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
index 2b472d25b..d638cafb8 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -19,17 +19,18 @@ namespace optcon {
  * \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>
+template <typename MANIFOLD, size_t CONTROL_DIM>
+class CostFunctionQuadraticSimple : public CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>
 {
 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;
+    using SCALAR = typename MANIFOLD::Scalar;
+    static constexpr size_t STATE_DIM = MANIFOLD::TangentDim;
 
-    typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+    typedef core::StateMatrix<STATE_DIM, SCALAR> state_matrix_t;
+    typedef core::ControlMatrix<CONTROL_DIM, SCALAR> control_matrix_t;
+    typedef core::ControlStateMatrix<STATE_DIM, CONTROL_DIM, SCALAR> control_state_matrix_t;
     typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
 
     /**
@@ -47,9 +48,9 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<STATE_DIM, CONT
      */
     CostFunctionQuadraticSimple(const state_matrix_t& Q,
         const control_matrix_t& R,
-        const state_vector_t& x_nominal,
+        const MANIFOLD& x_nominal,
         const control_vector_t& u_nominal,
-        const state_vector_t& x_final,
+        const MANIFOLD& x_final,
         const state_matrix_t& Q_final);
 
     virtual ~CostFunctionQuadraticSimple();
@@ -60,15 +61,13 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<STATE_DIM, CONT
      * Clones the cost function.
      * @return
      */
-    CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+    CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>* clone() const override;
 
-    virtual void setCurrentStateAndControl(const state_vector_t& x,
-        const control_vector_t& u,
-        const SCALAR& t) override;
+    virtual void setCurrentStateAndControl(const MANIFOLD& x, const control_vector_t& u, const SCALAR& t) override;
 
     virtual SCALAR evaluateIntermediate() override;
 
-    virtual state_vector_t stateDerivativeIntermediate() override;
+    virtual typename MANIFOLD::Tangent stateDerivativeIntermediate() override;
 
     virtual state_matrix_t stateSecondDerivativeIntermediate() override;
 
@@ -80,24 +79,24 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<STATE_DIM, CONT
 
     virtual SCALAR evaluateTerminal() override;
 
-    virtual state_vector_t stateDerivativeTerminal() override;
+    virtual typename MANIFOLD::Tangent stateDerivativeTerminal() override;
 
     virtual state_matrix_t stateSecondDerivativeTerminal() override;
 
-    virtual void updateReferenceState(const state_vector_t& x_ref) override;
+    virtual void updateReferenceState(const MANIFOLD& x_ref) override;
 
-    virtual void updateFinalState(const state_vector_t& x_final) override;
+    virtual void updateFinalState(const MANIFOLD& x_final) override;
 
 protected:
-    state_vector_t x_deviation_;
-    state_vector_t x_nominal_;
+    typename MANIFOLD::Tangent x_deviation_;
+    MANIFOLD 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_;
+    MANIFOLD x_final_;
     state_matrix_t Q_final_;
 };
 
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfun-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/costfun-impl.hpp
index f754364aa..d18265be3 100644
--- a/ct_optcon/include/ct/optcon/costfunction/costfun-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/costfun-impl.hpp
@@ -19,4 +19,4 @@
 #include "CostFunctionAD-impl.hpp"
 #include "CostFunctionAnalytical-impl.hpp"
 #include "CostFunctionQuadratic-impl.hpp"
-//#include "CostFunctionQuadraticSimple-impl.hpp"
+#include "CostFunctionQuadraticSimple-impl.hpp"
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfun.hpp b/ct_optcon/include/ct/optcon/costfunction/costfun.hpp
index cb95f56b4..aff87649f 100644
--- a/ct_optcon/include/ct/optcon/costfunction/costfun.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/costfun.hpp
@@ -19,4 +19,4 @@
 #include "CostFunctionQuadratic.hpp"
 #include "CostFunctionAD.hpp"
 #include "CostFunctionAnalytical.hpp"
-//#include "CostFunctionQuadraticSimple.hpp"
+#include "CostFunctionQuadraticSimple.hpp"
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
index 9ec54b6a4..41e48b550 100644
--- a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
@@ -9,17 +9,15 @@ namespace ct {
 namespace optcon {
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCBackendBase(
-    const OptConProblem_t& optConProblem,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::NLOCBackendBase(const OptConProblem_t& optConProblem,
     const Settings_t& settings)
     : NLOCBackendBase(createSystemInterface(optConProblem, settings), settings)
 {
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCBackendBase(
-    const OptConProblem_t& optConProblem,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::NLOCBackendBase(const OptConProblem_t& optConProblem,
     const std::string& settingsFile,
     bool verbose,
     const std::string& ns)
@@ -27,9 +25,8 @@ NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCB
 {
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCBackendBase(
-    const systemInterfacePtr_t& systemInterface,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::NLOCBackendBase(const systemInterfacePtr_t& systemInterface,
     const Settings_t& settings)
     : initialized_(false),
       configured_(false),
@@ -83,9 +80,8 @@ NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCB
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCBackendBase(
-    const systemInterfacePtr_t& systemInterface,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::NLOCBackendBase(const systemInterfacePtr_t& systemInterface,
     const std::string& settingsFile,
     bool verbose,
     const std::string& ns)
@@ -93,38 +89,31 @@ NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::NLOCB
 {
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::~NLOCBackendBase()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::~NLOCBackendBase()
 {
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-template <typename T>
-typename std::enable_if<std::is_same<T, ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::systemInterfacePtr_t>::type
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::createSystemInterface(
-    const OptConProblem_t& optConProblem,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+typename std::enable_if<TIME_T == core::TIME_TYPE::CONTINUOUS_TIME,
+    typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::systemInterfacePtr_t>::type
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::createSystemInterface(const OptConProblem_t& optConProblem,
     const Settings_t& settings)
 {
-    return systemInterfacePtr_t(
-        new OptconContinuousSystemInterface<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(optConProblem, settings));
+    return systemInterfacePtr_t(new OptconContinuousSystemInterface<MANIFOLD, CONTROL_DIM>(optConProblem, settings));
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-template <typename T>
-typename std::enable_if<std::is_same<T, DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::systemInterfacePtr_t>::type
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::createSystemInterface(
-    const OptConProblem_t& optConProblem,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+typename std::enable_if<TIME_T == core::TIME_TYPE::DISCRETE_TIME,
+    typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::systemInterfacePtr_t>::type
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::createSystemInterface(const OptConProblem_t& optConProblem,
     const Settings_t& settings)
 {
-    return systemInterfacePtr_t(
-        new OptconDiscreteSystemInterface<STATE_DIM, CONTROL_DIM, SCALAR>(optConProblem, settings));
+    return systemInterfacePtr_t(new OptconDiscreteSystemInterface<MANIFOLD, CONTROL_DIM>(optConProblem, settings));
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::setInitialGuess(
-    const Policy_t& initialGuess)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::setInitialGuess(const Policy_t& initialGuess)
 {
     if (initialGuess.x_ref().size() < (size_t)K_ + 1)
     {
@@ -186,8 +175,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     finalCostPrevious_ = finalCostBest_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeTimeHorizon(int numStages)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeTimeHorizon(int numStages)
 {
     if (numStages == K_)
         return;
@@ -225,8 +214,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     lqocSolver_->setProblem(lqocProblem_);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeTimeHorizon(const SCALAR& tf)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeTimeHorizon(const SCALAR& tf)
 {
     if (tf < 0)
         throw std::runtime_error("negative time horizon specified");
@@ -234,9 +223,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     changeTimeHorizon(settings_.computeK(tf));
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeInitialState(
-    const core::StateVector<STATE_DIM, SCALAR>& x0)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeInitialState(const MANIFOLD& x0)
 {
     if (x_.size() == 0)
         x_.resize(1);
@@ -245,8 +233,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeCostFunction(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeCostFunction(
     const typename OptConProblem_t::CostFunctionPtr_t& cf)
 {
     if (cf == nullptr)
@@ -266,15 +254,15 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
         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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeNonlinearSystem(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeNonlinearSystem(
     const typename OptConProblem_t::DynamicsPtr_t& dyn)
 {
     systemInterface_->changeNonlinearSystem(dyn);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeInputBoxConstraints(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeInputBoxConstraints(
     const typename OptConProblem_t::ConstraintPtr_t& con)
 {
     if (con == nullptr)
@@ -295,8 +283,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     setInputBoxConstraintsForLQOCProblem();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeStateBoxConstraints(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeStateBoxConstraints(
     const typename OptConProblem_t::ConstraintPtr_t& con)
 {
     if (con == nullptr)
@@ -318,8 +306,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeGeneralConstraints(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeGeneralConstraints(
     const typename OptConProblem_t::ConstraintPtr_t& con)
 {
     if (con == nullptr)
@@ -360,15 +348,15 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
         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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::changeLinearSystem(
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::changeLinearSystem(
     const typename OptConProblem_t::LinearPtr_t& lin)
 {
     systemInterface_->changeLinearSystem(lin);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::checkProblem()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::checkProblem()
 {
     if (K_ == 0)
         throw std::runtime_error("Time horizon too small resulting in 0 steps");
@@ -381,8 +369,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::configure(const Settings_t& settings)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::configure(const Settings_t& settings)
 {
     if (!settings.parametersOk())
     {
@@ -432,8 +420,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     configured_ = true;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::rolloutSingleShot(const size_t threadId,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::rolloutSingleShot(const size_t threadId,
     const size_t k,  //! the starting index of the shot
     ControlVectorArray& u_local,
     StateVectorArray& x_local,
@@ -520,9 +508,8 @@ bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     return true;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::rolloutShotsSingleThreaded(
-    size_t threadId,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::rolloutShotsSingleThreaded(size_t threadId,
     size_t firstIndex,
     size_t lastIndex,
     ControlVectorArray& u_ff_local,
@@ -552,8 +539,8 @@ bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     return true;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeSingleDefect(size_t k,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeSingleDefect(size_t k,
     const StateVectorArray& x_local,
     const StateVectorArray& xShot,
     StateVectorArray& d) const
@@ -568,10 +555,9 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     //! 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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeCostsOfTrajectory(
-    size_t threadId,
-    const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeCostsOfTrajectory(size_t threadId,
+    const ct::core::DiscreteArray<MANIFOLD>& x_local,
     const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
     scalar_t& intermediateCost,
     scalar_t& finalCost) const
@@ -592,10 +578,9 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     finalCost = costFunctions_[threadId]->evaluateTerminal();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeBoxConstraintErrorOfTrajectory(
-    size_t threadId,
-    const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeBoxConstraintErrorOfTrajectory(size_t threadId,
+    const ct::core::DiscreteArray<MANIFOLD>& x_local,
     const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
     scalar_t& e_tot) const
 {
@@ -642,12 +627,11 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
-    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
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeGeneralConstraintErrorOfTrajectory(size_t threadId,
+    const ct::core::DiscreteArray<MANIFOLD>& x_local,
+    const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+    scalar_t& e_tot) const
 {
     e_tot = 0;
 
@@ -681,9 +665,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::executeLQApproximation(size_t threadId,
-    size_t k)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::executeLQApproximation(size_t threadId, size_t k)
 {
     LQOCProblem_t& p = *lqocProblem_;
     const scalar_t& dt = settings_.dt;
@@ -723,8 +706,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::setInputBoxConstraintsForLQOCProblem()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::setInputBoxConstraintsForLQOCProblem()
 {
     // set box constraints if there are any
     if (inputBoxConstraints_[settings_.nThreads] != nullptr)
@@ -748,8 +731,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::setStateBoxConstraintsForLQOCProblem()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::setStateBoxConstraintsForLQOCProblem()
 {
     if (stateBoxConstraints_[settings_.nThreads] != nullptr)
     {
@@ -786,10 +769,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeLinearizedConstraints(
-    size_t threadId,
-    size_t k)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeLinearizedConstraints(size_t threadId, size_t k)
 {
     // set general if there are any
     if (generalConstraints_[threadId] != nullptr)
@@ -815,8 +796,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::initializeCostToGo()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::initializeCostToGo()
 {
     LQOCProblem_t& p = *lqocProblem_;
 
@@ -846,8 +827,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::printSummary()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::printSummary()
 {
     SCALAR d_norm_l1 = computeDefectsNorm<1>(d_);
     SCALAR d_norm_l2 = computeDefectsNorm<2>(d_);
@@ -891,8 +872,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::logToMatlab(const size_t& iteration)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::logToMatlab(const size_t& iteration)
 {
 #ifdef MATLAB_FULL_LOG
 
@@ -942,8 +923,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::logInitToMatlab()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::logInitToMatlab()
 {
 #ifdef MATLAB
     if (settings_.logToMatlab)
@@ -975,9 +956,9 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
 const core::ControlTrajectory<CONTROL_DIM, SCALAR>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getControlTrajectory() const
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getControlTrajectory() const
 {
     //! \todo this method currently copies the time array (suboptimal)
 
@@ -988,26 +969,25 @@ NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getCo
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const core::StateTrajectory<STATE_DIM, SCALAR>
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getStateTrajectory() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const core::DiscreteTrajectory<MANIFOLD> NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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_);
+    return core::DiscreteTrajectory<MANIFOLD>(t_control, x_);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getCost() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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 CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::lineSearch()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::lineSearch()
 {
     // lowest cost
     scalar_t lowestCostPrevious;
@@ -1103,12 +1083,12 @@ bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::executeLineSearch(const size_t threadId,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::executeLineSearch(const size_t threadId,
     const scalar_t alpha,
-    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::DiscreteArray<MANIFOLD>& x_alpha,
+    ct::core::DiscreteArray<MANIFOLD>& x_shot_alpha,
+    ct::core::DiscreteArray<MANIFOLD>& defects_recorded,
     ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_alpha,
     scalar_t& intermediateCost,
     scalar_t& finalCost,
@@ -1135,7 +1115,7 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     x_alpha = delta_x_ * alpha + x_prev_;
 
     // update x_lqr reference
-    ct::core::StateVectorArray<STATE_DIM, SCALAR> x_ref_lqr = delta_x_ref_lqr_ * alpha + x_prev_;
+    ct::core::DiscreteArray<MANIFOLD> x_ref_lqr = delta_x_ref_lqr_ * alpha + x_prev_;
 
     if (terminationFlag && *terminationFlag)
         return;
@@ -1183,8 +1163,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::acceptStep(const SCALAR alpha,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::acceptStep(const SCALAR alpha,
     const SCALAR intermediateCost,
     const SCALAR finalCost,
     const SCALAR defectNorm,
@@ -1276,8 +1256,8 @@ bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::prepareSolveLQProblem(size_t startIndex)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::prepareSolveLQProblem(size_t startIndex)
 {
     lqpCounter_++;
 
@@ -1300,8 +1280,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::finishSolveLQProblem(size_t endIndex)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::finishSolveLQProblem(size_t endIndex)
 {
     lqpCounter_++;
 
@@ -1324,8 +1304,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::solveFullLQProblem()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::solveFullLQProblem()
 {
     lqpCounter_++;
 
@@ -1334,17 +1314,16 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     lqocSolver_->solve();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::updateCosts()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::retrieveLastAffineModel(
-    StateMatrixArray& A,
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::retrieveLastAffineModel(StateMatrixArray& A,
     StateControlMatrixArray& B,
     StateVectorArray& b)
 {
@@ -1353,16 +1332,16 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     b = lqocProblem_->b_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::Policy_t&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getSolution()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::Policy_t&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::reset()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::reset()
 {
     firstRollout_ = true;
     iteration_ = 0;
@@ -1377,8 +1356,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::extractSolution()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::extractSolution()
 {
     L_.setConstant(core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());  // TODO should eventually go away
     x_ref_lqr_ = x_;
@@ -1402,7 +1381,7 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
             lqocSolver_->computeStatesAndControls();
             delta_u_ff_ = lqocSolver_->getSolutionControl();
             delta_x_ = lqocSolver_->getSolutionState();
-            delta_x_ref_lqr_.setConstant(ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+            delta_x_ref_lqr_.setConstant(MANIFOLD::NeutralElement());
             break;
         }
         case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
@@ -1411,15 +1390,15 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
             delta_u_ff_ = lqocSolver_->get_lv();
             lqocSolver_->computeFeedbackMatrices();
             L_ = lqocSolver_->getSolutionFeedback();
-            delta_x_.setConstant(ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
-            delta_x_ref_lqr_.setConstant(ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+            delta_x_.setConstant(MANIFOLD::NeutralElement());
+            delta_x_ref_lqr_.setConstant(MANIFOLD::NeutralElement());
             break;
         }
         case NLOptConSettings::NLOCP_ALGORITHM::MS_ILQR:
         {
             lqocSolver_->computeStatesAndControls();
             delta_x_ = lqocSolver_->getSolutionState();
-            delta_x_ref_lqr_.setConstant(ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+            delta_x_ref_lqr_.setConstant(MANIFOLD::NeutralElement());
             lqocSolver_->compute_lv();
             delta_u_ff_ = lqocSolver_->get_lv();
             lqocSolver_->computeFeedbackMatrices();
@@ -1443,8 +1422,8 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::doFullStepUpdate()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::doFullStepUpdate()
 {
     alphaBest_ = 1.0;
 
@@ -1453,44 +1432,42 @@ void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     x_ref_lqr_ += delta_x_ref_lqr_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::logSummaryToMatlab(
-    const std::string& fileName)
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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, bool CONTINUOUS>
-const SummaryAllIterations<SCALAR>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getSummary() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const SummaryAllIterations<SCALAR>& NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getSummary() const
 {
     return summaryAllIterations_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::resetDefects()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::resetDefects()
 {
     d_.setConstant(state_vector_t::Zero());
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDefectsNorm()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+void NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeDefectsNorm()
 {
     d_norm_ = computeDefectsNorm<1>(d_);
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-size_t& NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::iteration()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+size_t& NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::iteration()
 {
     return iteration_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::nominalRollout()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::nominalRollout()
 {
     bool success =
         rolloutSingleShot(settings_.nThreads, 0, u_ff_, x_, x_prev_, xShot_, *this->substepsX_, *this->substepsU_);
@@ -1500,163 +1477,151 @@ bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::
     return success;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::TimeArray&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getTimeArray()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::TimeArray&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getTimeArray()
 {
     return t_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::isConfigured()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::isConfigured()
 {
     return configured_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::isInitialized()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::isInitialized()
 {
     return initialized_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getTotalDefect() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::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 CONTINUOUS>
-bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::testConsistency()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+bool NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::testConsistency()
 {
     return true;  // TODO this is currently meaningless
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::DynamicsPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getNonlinearSystemsInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getNonlinearSystemsInstances()
 {
     return systemInterface_->getNonlinearSystemsInstances();
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::DynamicsPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getNonlinearSystemsInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getNonlinearSystemsInstances() const
 {
     return systemInterface_->getNonlinearSystemsInstances();
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::LinearPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getLinearSystemsInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getLinearSystemsInstances()
 {
     return systemInterface_->getLinearSystemsInstances();
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<
-    typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::LinearPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getLinearSystemsInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getLinearSystemsInstances() const
 {
     return systemInterface_->getLinearSystemsInstances();
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        CostFunctionPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getCostFunctionInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getCostFunctionInstances()
 {
     return costFunctions_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        CostFunctionPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getCostFunctionInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getCostFunctionInstances() const
 {
     return costFunctions_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getInputBoxConstraintsInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getInputBoxConstraintsInstances()
 {
     return inputBoxConstraints_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getInputBoxConstraintsInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getInputBoxConstraintsInstances() const
 {
     return inputBoxConstraints_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getStateBoxConstraintsInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getStateBoxConstraintsInstances()
 {
     return stateBoxConstraints_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getStateBoxConstraintsInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getStateBoxConstraintsInstances() const
 {
     return stateBoxConstraints_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getGeneralConstraintsInstances()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getGeneralConstraintsInstances()
 {
     return generalConstraints_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::OptConProblem_t::
-        ConstraintPtr_t>&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getGeneralConstraintsInstances() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const std::vector<typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getGeneralConstraintsInstances() const
 {
     return generalConstraints_;
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getTimeHorizon()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getTimeHorizon()
 {
     return K_ * settings_.dt;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getNumSteps()
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+int NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getNumSteps()
 {
     return K_;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getNumStepsPerShot() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+int NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getNumStepsPerShot() const
 {
     if (settings_.isSingleShooting())
         return K_;
@@ -1664,9 +1629,9 @@ int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::g
         return settings_.K_shot;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
-const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::Settings_t&
-NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::getSettings() const
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
+const typename NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::Settings_t&
+NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::getSettings() const
 {
     return settings_;
 }
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
index 7726909d5..f0559552a 100644
--- a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
@@ -39,32 +39,29 @@ namespace optcon {
  *
  */
 
-template <size_t STATE_DIM,
-    size_t CONTROL_DIM,
-    size_t P_DIM,
-    size_t V_DIM,
-    typename SCALAR = double,
-    bool CONTINUOUS = true>
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
 class NLOCBackendBase
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    static const size_t state_dim = STATE_DIM;
+    static const size_t STATE_DIM = MANIFOLD::TangentDim;
+    static const size_t state_dim = MANIFOLD::TangentDim;
     static const size_t control_dim = CONTROL_DIM;
+    using SCALAR = typename MANIFOLD::Scalar;
+    using scalar_t = SCALAR;
+    using scalar_array_t = std::vector<SCALAR, Eigen::aligned_allocator<SCALAR>>;
 
     typedef NLOptConSettings Settings_t;
 
-    typedef typename ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+    typedef typename ct::core::StateFeedbackController<MANIFOLD, CONTROL_DIM, TIME_T> Policy_t;
 
-    typedef typename std::conditional<CONTINUOUS,
-        ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>,
-        DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::type OptConProblem_t;
+    typedef OptConProblem<MANIFOLD, CONTROL_DIM, TIME_T> OptConProblem_t;
 
     typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
     typedef LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR> LQOCSolver_t;
 
-    typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+    typedef ct::core::DiscreteArray<MANIFOLD> StateVectorArray;
     typedef std::shared_ptr<StateVectorArray> StateVectorArrayPtr;
 
     typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
@@ -76,7 +73,7 @@ class NLOCBackendBase
     typedef std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>> ControlSubsteps;
     typedef std::shared_ptr<ControlSubsteps> ControlSubstepsPtr;
 
-    typedef OptconSystemInterface<STATE_DIM, CONTROL_DIM, OptConProblem_t, SCALAR> systemInterface_t;
+    typedef OptconSystemInterface<MANIFOLD, CONTROL_DIM, TIME_T> systemInterface_t;
     typedef std::shared_ptr<systemInterface_t> systemInterfacePtr_t;
 
     using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM, SCALAR>;
@@ -86,17 +83,13 @@ class NLOCBackendBase
     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 state_matrix_t = ct::core::StateMatrix<STATE_DIM, SCALAR>;
+    using control_matrix_t = ct::core::ControlMatrix<CONTROL_DIM, SCALAR>;
+    using control_state_matrix_t = ct::core::ControlStateMatrix<STATE_DIM, CONTROL_DIM, SCALAR>;
+    using state_control_matrix_t = ct::core::StateControlMatrix<STATE_DIM, CONTROL_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_t& optConProblem, const Settings_t& settings);
     NLOCBackendBase(const OptConProblem_t& optConProblem,
@@ -112,21 +105,18 @@ class NLOCBackendBase
 
     virtual ~NLOCBackendBase();
 
-    template <typename T = OptConProblem_t>  // do not use this template argument
-    typename std::enable_if<std::is_same<T, ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
-        systemInterfacePtr_t>::type
+    typename std::enable_if<TIME_T == core::TIME_TYPE::CONTINUOUS_TIME, systemInterfacePtr_t>::type
     createSystemInterface(const OptConProblem_t& optConProblem, const Settings_t& settings);
 
-    template <typename T = OptConProblem_t>  // do not use this template argument
-    typename std::enable_if<std::is_same<T, DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
-        systemInterfacePtr_t>::type
-    createSystemInterface(const OptConProblem_t& optConProblem, const Settings_t& settings);
+    typename std::enable_if<TIME_T == core::TIME_TYPE::DISCRETE_TIME, systemInterfacePtr_t>::type createSystemInterface(
+        const OptConProblem_t& optConProblem,
+        const Settings_t& settings);
 
     //! configure the solver
     /**
-     * Configure the solver
-     * @param settings solver settings
-     */
+     * @brief solver settings 
+     * 
+     */ 
     virtual void configure(const Settings_t& settings);
 
     //! get the current SLQsolver settings
@@ -283,7 +273,7 @@ class NLOCBackendBase
 
     void reset();
 
-    const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const;
+    const core::DiscreteTrajectory<MANIFOLD> getStateTrajectory() const;
 
     const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const;
 
@@ -466,7 +456,7 @@ class NLOCBackendBase
      * \param finalCost the accumulated final cost
      */
     void computeCostsOfTrajectory(size_t threadId,
-        const core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+        const core::DiscreteArray<MANIFOLD>& x_local,
         const core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
         scalar_t& intermediateCost,
         scalar_t& finalCost) const;
@@ -480,7 +470,7 @@ class NLOCBackendBase
      * \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::DiscreteArray<MANIFOLD>& x_local,
         const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
         scalar_t& e_tot) const;
 
@@ -493,16 +483,16 @@ class NLOCBackendBase
      * \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::DiscreteArray<MANIFOLD>& 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 executeLineSearch(const size_t threadId,
         const scalar_t alpha,
-        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::DiscreteArray<MANIFOLD>& x_recorded,
+        ct::core::DiscreteArray<MANIFOLD>& x_shot_recorded,
+        ct::core::DiscreteArray<MANIFOLD>& defects_recorded,
         ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_recorded,
         scalar_t& intermediateCost,
         scalar_t& finalCost,
@@ -515,8 +505,7 @@ class NLOCBackendBase
 
 
     //! in case of line-search compute new merit and check if to accept step. Returns true if accept step
-    bool acceptStep(
-        const SCALAR alpha,
+    bool acceptStep(const SCALAR alpha,
         const SCALAR intermediateCost,
         const SCALAR finalCost,
         const SCALAR defectNorm,
@@ -619,17 +608,16 @@ class NLOCBackendBase
 
     SummaryAllIterations<SCALAR> summaryAllIterations_;
 
-    //! if building with MATLAB support, include matfile
+//! if building with MATLAB support, include matfile
 #ifdef MATLAB
     matlab::MatFile matFile_;
 #endif  //MATLAB
 };
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
 template <typename ARRAY_TYPE, size_t ORDER>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDiscreteArrayNorm(
-    const ARRAY_TYPE& d) const
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeDiscreteArrayNorm(const ARRAY_TYPE& d) const
 {
     SCALAR norm = 0.0;
 
@@ -641,10 +629,9 @@ SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>
 }
 
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
 template <typename ARRAY_TYPE, size_t ORDER>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDiscreteArrayNorm(
-    const ARRAY_TYPE& a,
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeDiscreteArrayNorm(const ARRAY_TYPE& a,
     const ARRAY_TYPE& b) const
 {
     assert(a.size() == b.size());
@@ -658,10 +645,9 @@ SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>
     return norm;
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
+template <typename MANIFOLD, size_t CONTROL_DIM, ct::core::TIME_TYPE TIME_T>
 template <size_t ORDER>
-SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDefectsNorm(
-    const StateVectorArray& d) const
+SCALAR NLOCBackendBase<MANIFOLD, CONTROL_DIM, TIME_T>::computeDefectsNorm(const StateVectorArray& d) const
 {
     return computeDiscreteArrayNorm<StateVectorArray, ORDER>(d);
 }
diff --git a/ct_optcon/include/ct/optcon/optcon.h b/ct_optcon/include/ct/optcon/optcon.h
index b10f29178..631b09c79 100644
--- a/ct_optcon/include/ct/optcon/optcon.h
+++ b/ct_optcon/include/ct/optcon/optcon.h
@@ -13,7 +13,7 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 //#include "constraint/constraint.h"
 
 #include "problem/OptConProblem.h"
-//#include "problem/LQOCProblem.hpp"
+#include "problem/LQOCProblem.hpp"
 #include "solver/NLOptConSettings.hpp"
 
 #include "system_interface/OptconSystemInterface.h"
@@ -27,8 +27,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 //#include "nloc/algorithms/SingleShooting.hpp"
 //
 //#include "solver/OptConSolver.h"
-//#include "solver/lqp/HPIPMInterface.hpp"
-//#include "solver/lqp/GNRiccatiSolver.hpp"
+#include "solver/lqp/HPIPMInterface.hpp"
+#include "solver/lqp/GNRiccatiSolver.hpp"
 //#include "solver/NLOptConSolver.hpp"
 //
 //#include "lqr/riccati/CARE.hpp"
@@ -52,13 +52,13 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 //#include "constraint/constraint-impl.h"
 //
 #include "problem/OptConProblem-impl.h"
-//#include "problem/LQOCProblem-impl.hpp"
+#include "problem/LQOCProblem-impl.hpp"
 
 #include "system_interface/OptconContinuousSystemInterface-impl.h"
 #include "system_interface/OptconDiscreteSystemInterface-impl.h"
 
-//#include "solver/lqp/GNRiccatiSolver-impl.hpp"
-//#include "solver/lqp/HPIPMInterface-impl.hpp"
+#include "solver/lqp/GNRiccatiSolver-impl.hpp"
+#include "solver/lqp/HPIPMInterface-impl.hpp"
 //#include "solver/NLOptConSolver-impl.hpp"
 //
 //#include "lqr/riccati/CARE-impl.hpp"
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
index 9fd043145..c9bb43b73 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -8,20 +8,24 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 namespace ct {
 namespace optcon {
 
-
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LQOCProblem(int N) : nbu_(N, 0), nbx_(N + 1, 0)
+template <typename MANIFOLD, int CONTROL_DIM>
+LQOCProblem<MANIFOLD, CONTROL_DIM>::LQOCProblem(int N) : nbu_(N, 0), nbx_(N + 1, 0)
 {
     changeNumStages(N);
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isConstrained() const
+template <typename MANIFOLD, int CONTROL_DIM>
+LQOCProblem<MANIFOLD, CONTROL_DIM>::~LQOCProblem()
+{
+}
+
+template <typename MANIFOLD, int CONTROL_DIM>
+bool LQOCProblem<MANIFOLD, CONTROL_DIM>::isConstrained() const
 {
     return (isInputBoxConstrained() | isStateBoxConstrained() | isGeneralConstrained());
 }
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isInputBoxConstrained() const
+template <typename MANIFOLD, int CONTROL_DIM>
+bool LQOCProblem<MANIFOLD, CONTROL_DIM>::isInputBoxConstrained() const
 {
     if (std::accumulate(nbu_.begin(), nbu_.end(), 0) > 0)
         return true;
@@ -29,8 +33,8 @@ bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isInputBoxConstrained() const
     return false;
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isStateBoxConstrained() const
+template <typename MANIFOLD, int CONTROL_DIM>
+bool LQOCProblem<MANIFOLD, CONTROL_DIM>::isStateBoxConstrained() const
 {
     if (std::accumulate(nbx_.begin(), nbx_.end(), 0) > 0)
         return true;
@@ -38,8 +42,8 @@ bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isStateBoxConstrained() const
     return false;
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isGeneralConstrained() const
+template <typename MANIFOLD, int CONTROL_DIM>
+bool LQOCProblem<MANIFOLD, CONTROL_DIM>::isGeneralConstrained() const
 {
     if (std::accumulate(ng_.begin(), ng_.end(), 0) > 0)
         return true;
@@ -47,14 +51,14 @@ bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isGeneralConstrained() const
     return false;
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-int LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getNumberOfStages()
+template <typename MANIFOLD, int CONTROL_DIM>
+int LQOCProblem<MANIFOLD, CONTROL_DIM>::getNumberOfStages()
 {
     return K_;
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumStages(int N)
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::changeNumStages(int N)
 {
     K_ = N;
 
@@ -66,6 +70,7 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumStages(int N)
     q_.resize(N + 1);
     qv_.resize(N + 1);
     Q_.resize(N + 1);
+    Acal_.resize(N+1);
 
     rv_.resize(N);
     R_.resize(N);
@@ -88,8 +93,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumStages(int N)
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setZero(const int& nGenConstr)
 {
     A_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Zero());
     B_.setConstant(core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());
@@ -97,6 +102,7 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
     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());
+    Acal_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Identity());
     rv_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
     R_.setConstant(core::ControlMatrix<CONTROL_DIM, SCALAR>::Zero());
     q_.setConstant((SCALAR)0.0);
@@ -125,8 +131,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInputBoxConstraint(const int index,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setInputBoxConstraint(const int index,
     const int nConstr,
     const constr_vec_t& u_lb,
     const constr_vec_t& u_ub,
@@ -157,8 +163,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInputBoxConstraint(const in
     }
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInputBoxConstraints(const int nConstr,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setInputBoxConstraints(const int nConstr,
     const constr_vec_t& u_lb,
     const constr_vec_t& u_ub,
     const VectorXi& sp,
@@ -169,8 +175,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInputBoxConstraints(const i
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateStateBoxConstraint(const int index,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setIntermediateStateBoxConstraint(const int index,
     const int nConstr,
     const constr_vec_t& x_lb,
     const constr_vec_t& x_ub,
@@ -201,8 +207,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateStateBoxConstra
     }
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateStateBoxConstraints(const int nConstr,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setIntermediateStateBoxConstraints(const int nConstr,
     const constr_vec_t& x_lb,
     const constr_vec_t& x_ub,
     const VectorXi& sp,
@@ -213,8 +219,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateStateBoxConstra
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTerminalBoxConstraints(const int nConstr,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setTerminalBoxConstraints(const int nConstr,
     const constr_vec_t& x_lb,
     const constr_vec_t& x_ub,
     const VectorXi& sp,
@@ -244,8 +250,8 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTerminalBoxConstraints(cons
     }
 }
 
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const constr_vec_t& d_lb,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::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)
@@ -266,28 +272,23 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const co
     D_.setConstant(D);
 }
 
-
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
-void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuadraticProblem(
-    ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
-    ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
-    const ct::core::StateVector<STATE_DIM, SCALAR>& offset,
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProblem(const MANIFOLD& x0,
+    const core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+    ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
+    ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
+    const typename MANIFOLD::Tangent& stateOffset,
     const double dt)
 {
     setZero();
 
-    core::StateVector<STATE_DIM, SCALAR> x0;
-    x0.setZero();  // by definition
-    core::ControlVector<CONTROL_DIM, SCALAR> u0;
-    u0.setZero();  // by definition
-
     core::StateMatrix<STATE_DIM, SCALAR> A;
     core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
-    linearSystem.getAandB(x0, u0, 0, A, B);
+    linearSystem.getDerivatives(A, B, x0, u0, 0);
 
     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, offset);
+    b_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, stateOffset);
 
     // feed current state and control to cost function
     costFunction.setCurrentStateAndControl(x0, u0, 0);
@@ -298,12 +299,52 @@ void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuad
         core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, costFunction.stateControlDerivativeIntermediate() * dt);
     R_ = core::ControlMatrixArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlSecondDerivativeIntermediate() * dt);
 
-    qv_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, costFunction.stateDerivativeIntermediate() * dt);
+    qv_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, costFunction.stateDerivativeIntermediate() * dt);
     rv_ = core::ControlVectorArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlDerivativeIntermediate() * dt);
 
     // final stage
-    Q_[K_] = costFunction.stateSecondDerivativeTerminal();
-    qv_[K_] = costFunction.stateDerivativeTerminal();
+    Q_[K_] = costFunction.stateSecondDerivativeTerminal() * dt;
+    qv_[K_] = costFunction.stateDerivativeTerminal() * dt;
+}
+
+
+template <typename MANIFOLD, int CONTROL_DIM>
+void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProblem(
+    const ct::core::DiscreteArray<MANIFOLD>& x_traj,
+    const ct::core::DiscreteArray<core::ControlVector<CONTROL_DIM, SCALAR>>& u_traj,
+    ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
+    ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
+    const typename MANIFOLD::Tangent& stateOffset,
+    const double dt)
+{
+    setZero();
+
+    // get LTI dynamics
+    core::StateMatrix<STATE_DIM, SCALAR> A;
+    core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
+    linearSystem.getDerivatives(A, B, x_traj.front(), u_traj.front(), 0);
+    A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
+    B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
+    b_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, stateOffset);
+
+    // eval costs around the reference trajectory
+    for (int i = 0; i < K_; i++)
+    {
+        // feed current state and control to cost function
+        costFunction.setCurrentStateAndControl(x_traj[i], u_traj[i], i * dt);
+
+        // intermediate stage
+        Q_[i] = costFunction.stateSecondDerivativeIntermediate() * dt;
+        P_[i] = costFunction.stateControlDerivativeIntermediate() * dt;
+        R_[i] = costFunction.controlSecondDerivativeIntermediate() * dt;
+
+        qv_[i] = costFunction.stateDerivativeIntermediate() * dt;
+        rv_[i] = costFunction.controlDerivativeIntermediate() * dt;
+    }
+    // final stage
+    costFunction.setCurrentStateAndControl(x_traj.back(), u_traj.back(), K_ * dt);
+    Q_[K_] = costFunction.stateSecondDerivativeTerminal() * dt;
+    qv_[K_] = costFunction.stateDerivativeTerminal() * dt;
 }
 
 }  // namespace optcon
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
index 631b8d79d..df5407d33 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
@@ -53,12 +53,15 @@ namespace optcon {
  *
  * \todo Refactor the initializing methods such that const-references can be handed over.
  */
-template <int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
+template <typename MANIFOLD, int CONTROL_DIM>
 class LQOCProblem
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+    using SCALAR = typename MANIFOLD::Scalar;
+    static constexpr size_t STATE_DIM = MANIFOLD::TangentDim;
+
     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>;
@@ -84,6 +87,8 @@ class LQOCProblem
     //! constructor
     LQOCProblem(int N = 0);
 
+    ~LQOCProblem();
+
     //! returns the number of discrete time steps in the LQOCP, including terminal stage
     int getNumberOfStages();
 
@@ -189,10 +194,17 @@ class LQOCProblem
      * @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::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
-        ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
-        const ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
+    void setFromTimeInvariantLinearQuadraticProblem(const MANIFOLD& x0,
+        const core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+        ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
+        ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
+        const typename MANIFOLD::Tangent& stateOffset,
+        const double dt);
+    void setFromTimeInvariantLinearQuadraticProblem(const ct::core::DiscreteArray<MANIFOLD>& x_traj,
+        const ct::core::DiscreteArray<core::ControlVector<CONTROL_DIM, SCALAR>>& u_traj,
+        ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
+        ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
+        const typename MANIFOLD::Tangent& stateOffset,
         const double dt);
 
     //! return a flag indicating whether this LQOC Problem is constrained or not
@@ -205,14 +217,15 @@ class LQOCProblem
     //! 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_;
+    ct::core::DiscreteArray<typename MANIFOLD::Tangent> b_;
 
     //! 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::DiscreteArray<typename MANIFOLD::Tangent> qv_;
     ct::core::StateMatrixArray<STATE_DIM, SCALAR> Q_;
+    ct::core::StateMatrixArray<STATE_DIM, SCALAR> Acal_; // TODO: temp
 
     //! LQ approximation of the pure control penalty
     ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> rv_;
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
new file mode 100644
index 000000000..39591d47b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
@@ -0,0 +1,293 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
+Licensed under the BSD-2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::AugGNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
+    : LQOCSolver<MANIFOLD, CONTROL_DIM>(lqocProblem), N_(-1)
+{
+    Eigen::initParallel();
+    Eigen::setNbThreads(settings_.nThreadsEigen);
+}
+
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::AugGNRiccatiSolver(int N)
+{
+    changeNumberOfStages(N);
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solve()
+{
+    for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
+        solveSingleStage(i);
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int N)
+{
+    if (N == this->lqocProblem_->getNumberOfStages() - 1)
+        initializeCostToGo();
+
+    designController(N);
+
+    if (N > 0)
+        computeCostToGo(N);
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::configure(const NLOptConSettings& settings)
+{
+    settings_ = settings;
+    H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
+{
+    LQOCProblem_t& p = *this->lqocProblem_;
+
+    this->x_sol_[0].setZero();  // should always be zero (fixed init state)
+
+    for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+    {
+        //! control update rule in diff coordinates
+        this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
+
+        //! state update rule in diff coordinates
+        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];  // TODO: transport
+    }
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeFeedbackMatrices()
+{ /*no action required, already computed in backward pass */
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::compute_lv()
+{ /*no action required, already computed in backward pass*/
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::getSmallestEigenvalue() -> SCALAR
+{
+    return smallestEigenvalue_;
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
+{
+    if (lqocProblem->isConstrained())
+    {
+        throw std::runtime_error(
+            "Selected wrong solver - AugGNRiccatiSolver cannot handle constrained problems. Use a different solver");
+    }
+
+    const int& N = lqocProblem->getNumberOfStages();
+    changeNumberOfStages(N);
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::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);
+
+    this->lv_.resize(N);
+    this->L_.resize(N);
+
+    this->x_sol_.resize(N + 1);
+    this->u_sol_.resize(N);
+
+    sv_.resize(N + 1);
+    S_.resize(N + 1);
+    sv_tilda_.resize(N + 1);
+    S_tilda_.resize(N + 1);
+
+    N_ = N;
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::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];
+
+    S_tilda_[N] = p.Acal_[N] * S_[N] * p.Acal_[N].transpose();
+    sv_tilda_[N] = p.Acal_[N] * sv_[N];
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
+{
+    LQOCProblem_t& p = *this->lqocProblem_;
+
+    S_[k] = p.Q_[k];
+    S_[k].noalias() += p.A_[k].transpose() * S_tilda_[k + 1] * p.A_[k];
+    S_[k].noalias() -= this->L_[k].transpose() * Hi_[k] * this->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_tilda_[k + 1];
+    sv_[k].noalias() += p.A_[k].transpose() * S_tilda_[k + 1] * p.b_[k];
+    sv_[k].noalias() += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
+    sv_[k].noalias() += this->L_[k].transpose() * gv_[k];
+    sv_[k].noalias() += G_[k].transpose() * this->lv_[k];
+
+    S_tilda_[k] = p.Acal_[k] * S_[k] * p.Acal_[k].transpose();
+    sv_tilda_[k] = p.Acal_[k] * sv_[k];
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
+{
+    LQOCProblem_t& p = *this->lqocProblem_;
+
+    gv_[k] = p.rv_[k];
+    gv_[k].noalias() += p.B_[k].transpose() * sv_tilda_[k + 1];
+    //gv_[k].noalias() += p.B_[k].transpose() * S_tilda_[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_tilda_[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_tilda_[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
+        this->L_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * G_[k];
+
+        // calculate FF update
+        this->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
+        this->L_[k].noalias() = Hi_inverse_[k] * G_[k];
+
+        // calculate FF update
+        this->lv_[k].noalias() = Hi_inverse_[k] * gv_[k];
+    }
+}
+
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::logToMatlab()
+{
+#ifdef MATLAB_FULL_LOG
+
+    matFile_.open("AugGNRiccatiSolver.mat");
+
+    matFile_.put("sv", sv_.toImplementation());
+    matFile_.put("S", S_.toImplementation());
+    matFile_.put("L", this->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 <typename MANIFOLD, size_t CONTROL_DIM>
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeAndAllocate()
+{
+    // do nothing
+}
+
+}  // namespace optcon
+}  // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
new file mode 100644
index 000000000..846c251a5
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
@@ -0,0 +1,111 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
+Licensed under the BSD-2 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 <typename MANIFOLD, size_t CONTROL_DIM>
+class AugGNRiccatiSolver : public LQOCSolver<MANIFOLD, CONTROL_DIM>
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    static const int STATE_DIM = MANIFOLD::TangentDim;
+    static const int state_dim = STATE_DIM;
+    static const int control_dim = CONTROL_DIM;
+
+    using Base = LQOCSolver<MANIFOLD, CONTROL_DIM>;
+    using SCALAR = typename Base::SCALAR;
+    typedef typename Base::LQOCProblem_t 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::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+
+    AugGNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr);
+
+    AugGNRiccatiSolver(int N);
+
+    virtual void solve() override;
+
+    virtual void initializeAndAllocate() override;
+
+    virtual void solveSingleStage(int N) override;
+
+    virtual void configure(const NLOptConSettings& settings) override;
+
+    virtual void computeStatesAndControls() override;
+
+    virtual void computeFeedbackMatrices() override;
+
+    virtual void compute_lv() 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_;
+
+    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_;
+    StateMatrixArray S_;
+    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_tilda_;
+    StateMatrixArray S_tilda_;
+
+    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/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 74021f1ef..9f49c2d46 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -9,31 +9,30 @@ 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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
+    : LQOCSolver<MANIFOLD, CONTROL_DIM>(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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::GNRiccatiSolver(int N)
 {
     changeNumberOfStages(N);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solve()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solve()
 {
     for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
         solveSingleStage(i);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int N)
 {
     if (N == this->lqocProblem_->getNumberOfStages() - 1)
         initializeCostToGo();
@@ -44,16 +43,15 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
         computeCostToGo(N);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::configure(const NLOptConSettings& settings)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::configure(const NLOptConSettings& settings)
 {
     settings_ = settings;
     H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStatesAndControls()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -69,24 +67,24 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStatesAndControls()
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeFeedbackMatrices()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeFeedbackMatrices()
 { /*no action required, already computed in backward pass */
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::compute_lv()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::compute_lv()
 { /*no action required, already computed in backward pass*/
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-SCALAR GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSmallestEigenvalue()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::getSmallestEigenvalue() -> SCALAR
 {
     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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     if (lqocProblem->isConstrained())
     {
@@ -98,9 +96,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::setProblemImpl(std::shared
     changeNumberOfStages(N);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::changeNumberOfStages(int N)
 {
     if (N <= 0)
         return;
@@ -127,9 +124,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N
     N_ = N;
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeCostToGo()
 {
     //! since intializeCostToGo is the first call, we initialize the smallestEigenvalue here.
     smallestEigenvalue_ = std::numeric_limits<SCALAR>::infinity();
@@ -142,9 +138,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
     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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -162,9 +157,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeCostToGo(size_t k)
     sv_[k].noalias() += G_[k].transpose() * this->lv_[k];
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -261,9 +255,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
     }
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::logToMatlab()
 {
 #ifdef MATLAB_FULL_LOG
 
@@ -282,12 +275,11 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
 #endif
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeAndAllocate()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::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
index c6d59a1e5..7caf75f45 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
@@ -18,16 +18,19 @@ 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>
+template <typename MANIFOLD, size_t CONTROL_DIM>
+class GNRiccatiSolver : public LQOCSolver<MANIFOLD, CONTROL_DIM>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+    static const int STATE_DIM = MANIFOLD::TangentDim;
     static const int state_dim = STATE_DIM;
     static const int control_dim = CONTROL_DIM;
 
-    typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+    using Base = LQOCSolver<MANIFOLD, CONTROL_DIM>;
+    using SCALAR = typename Base::SCALAR;
+    typedef typename Base::LQOCProblem_t LQOCProblem_t;
 
     typedef ct::core::StateMatrix<STATE_DIM, SCALAR> StateMatrix;
     typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> StateMatrixArray;
@@ -36,8 +39,6 @@ class GNRiccatiSolver : public LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>
     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);
@@ -87,7 +88,7 @@ class GNRiccatiSolver : public LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>
     ControlMatrixArray Hi_inverse_;
     ControlMatrix H_corrFix_;
 
-    StateVectorArray sv_;
+    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_;
     StateMatrixArray S_;
 
     int N_;
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
index c1f018ed0..929b9c55c 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -10,8 +10,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 namespace ct {
 namespace optcon {
 
-template <int STATE_DIM, int CONTROL_DIM>
-HPIPMInterface<STATE_DIM, CONTROL_DIM>::HPIPMInterface()
+template <typename MANIFOLD, int CONTROL_DIM>
+HPIPMInterface<MANIFOLD, CONTROL_DIM>::HPIPMInterface()
     : N_(-1),
       settings_(NLOptConSettings()),
       dim_mem_(nullptr),
@@ -26,14 +26,14 @@ HPIPMInterface<STATE_DIM, CONTROL_DIM>::HPIPMInterface()
     configure(settings_);
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-HPIPMInterface<STATE_DIM, CONTROL_DIM>::~HPIPMInterface()
+template <typename MANIFOLD, int CONTROL_DIM>
+HPIPMInterface<MANIFOLD, CONTROL_DIM>::~HPIPMInterface()
 {
     freeHpipmMemory();
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::initializeAndAllocate()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::initializeAndAllocate()
 {
     freeHpipmMemory();
 
@@ -91,8 +91,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::initializeAndAllocate()
     }
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::freeHpipmMemory()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::freeHpipmMemory()
 {
     free(dim_mem_);
     free(qp_mem_);
@@ -107,16 +107,16 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::freeHpipmMemory()
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configure(const NLOptConSettings& settings)
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::configure(const NLOptConSettings& settings)
 {
     settings_ = settings;
     ::d_ocp_qp_ipm_arg_set_iter_max(&settings_.lqoc_solver_settings.num_lqoc_iterations, &arg_);
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::solve()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::solve()
 {
 // optional printout
 #ifdef HPIPM_PRINT_MATRICES
@@ -212,8 +212,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::solve()
     }
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeStatesAndControls()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
 {
     // extract solution for x and u
     for (int ii = 0; ii < N_; ii++)
@@ -234,9 +234,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeStatesAndControls()
     }
 }
 
-
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeFeedbackMatrices()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::computeFeedbackMatrices()
 {
     // extract data which is mandatory for computing either feedback or iLQR feedforward
     Eigen::Matrix<double, control_dim, control_dim> Lr;
@@ -244,7 +243,7 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeFeedbackMatrices()
     ct::core::ControlMatrix<control_dim> Lr0_inv =
         (Lr.template triangularView<Eigen::Lower>()).solve(Eigen::Matrix<double, control_dim, control_dim>::Identity());
 
-    LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+    LQOCProblem_t& p = *this->lqocProblem_;
     this->L_.resize(p.getNumberOfStages());
 
     for (int i = 1; i < this->lqocProblem_->getNumberOfStages(); i++)
@@ -266,15 +265,15 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeFeedbackMatrices()
     this->L_[0] = (-Lr0_inv.transpose() * Lr0_inv * G);
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::compute_lv()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::compute_lv()
 {
     for (int i = 0; i < this->lqocProblem_->getNumberOfStages(); i++)
         ::d_ocp_qp_ipm_get_ric_k(&qp_, &arg_, &workspace_, i, this->lv_[i].data());
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::printSolution()
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::printSolution()
 {
 #ifdef HPIPM_PRINT_MATRICES
     int ii;
@@ -299,9 +298,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::printSolution()
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setProblemImpl(
-    std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     // check if the number of stages N changed and adapt problem dimensions
     bool dimsChanged = changeProblemSize(lqocProblem);
@@ -329,9 +327,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setProblemImpl(
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureInputBoxConstraints(
-    std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+template <typename MANIFOLD, int CONTROL_DIM>
+bool HPIPMInterface<MANIFOLD, CONTROL_DIM>::configureInputBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     bool configChanged = false;
 
@@ -366,9 +363,8 @@ bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureInputBoxConstraints(
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureStateBoxConstraints(
-    std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+template <typename MANIFOLD, int CONTROL_DIM>
+bool HPIPMInterface<MANIFOLD, CONTROL_DIM>::configureStateBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     bool configChanged = false;
 
@@ -410,9 +406,8 @@ bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureStateBoxConstraints(
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureGeneralConstraints(
-    std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+template <typename MANIFOLD, int CONTROL_DIM>
+bool HPIPMInterface<MANIFOLD, CONTROL_DIM>::configureGeneralConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     bool configChanged = false;
 
@@ -466,12 +461,12 @@ bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureGeneralConstraints(
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setupCostAndDynamics(StateMatrixArray& A,
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::setupCostAndDynamics(StateMatrixArray& A,
     StateControlMatrixArray& B,
-    StateVectorArray& b,
+    core::DiscreteArray<typename MANIFOLD::Tangent>& b,
     FeedbackArray& P,
-    StateVectorArray& qv,
+    core::DiscreteArray<typename MANIFOLD::Tangent>& qv,
     StateMatrixArray& Q,
     ControlVectorArray& rv,
     ControlMatrixArray& R)
@@ -512,8 +507,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setupCostAndDynamics(StateMatrixArr
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::changeProblemSize(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> p)
+template <typename MANIFOLD, int CONTROL_DIM>
+bool HPIPMInterface<MANIFOLD, CONTROL_DIM>::changeProblemSize(std::shared_ptr<LQOCProblem_t> p)
 {
     const int N = p->getNumberOfStages();
 
@@ -598,8 +593,8 @@ bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::changeProblemSize(std::shared_ptr<L
     return true;
 }
 
-template <int STATE_DIM, int CONTROL_DIM>
-void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_mat(int m, int n, double* A, int lda)
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::d_print_mat(int m, int n, double* A, int lda)
 {
     int i, j;
     for (i = 0; i < m; i++)
@@ -614,8 +609,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_mat(int m, int n, double* A
 }
 
 
-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)
+template <typename MANIFOLD, int CONTROL_DIM>
+void HPIPMInterface<MANIFOLD, CONTROL_DIM>::d_print_e_tran_mat(int row, int col, double* A, int lda)
 {
     int i, j;
     for (j = 0; j < col; j++)
@@ -630,8 +625,8 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_e_tran_mat(int row, int col
 }
 
 
-template <int STATE_DIM, int CONTROL_DIM>
-const ct::core::ControlVectorArray<CONTROL_DIM>& HPIPMInterface<STATE_DIM, CONTROL_DIM>::get_lv()
+template <typename MANIFOLD, int CONTROL_DIM>
+const ct::core::ControlVectorArray<CONTROL_DIM>& HPIPMInterface<MANIFOLD, CONTROL_DIM>::get_lv()
 {
     return this->lv_;
 }
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
index f44dc5a22..b3a263199 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
@@ -37,14 +37,20 @@ namespace optcon {
  * 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>
+template <typename MANIFOLD, int CONTROL_DIM>
+class HPIPMInterface : public LQOCSolver<MANIFOLD, CONTROL_DIM>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    static const int state_dim = STATE_DIM;
-    static const int control_dim = CONTROL_DIM;
+    static constexpr size_t STATE_DIM = MANIFOLD::TangentDim;
+    static constexpr int state_dim = STATE_DIM;
+    static constexpr int control_dim = CONTROL_DIM;
+
+    using Base = LQOCSolver<MANIFOLD, CONTROL_DIM>;
+    using SCALAR = typename Base::SCALAR;
+    typedef typename Base::LQOCProblem_t LQOCProblem_t;
+
     static const int max_box_constr_dim = STATE_DIM + CONTROL_DIM;
 
     using StateMatrix = ct::core::StateMatrix<STATE_DIM>;
@@ -81,13 +87,11 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
     void printSolution();
 
     //! brief setup and configure the box constraints
-    virtual bool configureInputBoxConstraints(
-        std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
-    virtual bool configureStateBoxConstraints(
-        std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+    virtual bool configureInputBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
+    virtual bool configureStateBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
 
     //! brief setup and configure the general (in)equality constraints
-    virtual bool configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+    virtual bool configureGeneralConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
 
     /*!
      * @brief allocate memory for HPIPM
@@ -116,7 +120,7 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
      *
      * \warning If you wish to
      */
-    void setProblemImpl(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+    void setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
 
     /*!
      * @brief transcribe the problem for HPIPM
@@ -139,9 +143,9 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
      */
     void setupCostAndDynamics(StateMatrixArray& A,
         StateControlMatrixArray& B,
-        StateVectorArray& b,
+        core::DiscreteArray<typename MANIFOLD::Tangent>& b,
         FeedbackArray& P,
-        StateVectorArray& qv,
+        core::DiscreteArray<typename MANIFOLD::Tangent>& qv,
         StateMatrixArray& Q,
         ControlVectorArray& rv,
         ControlMatrixArray& R);
@@ -150,7 +154,7 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
      * @brief change the size of the optimal control problem
      * @return true if the problem size changed, otherwise return false
      */
-    bool changeProblemSize(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem);
+    bool changeProblemSize(std::shared_ptr<LQOCProblem_t> lqocProblem);
 
     //! prints a matrix in column-major format
     void d_print_mat(int m, int n, double* A, int lda);
@@ -179,7 +183,7 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
     std::vector<double*> hA_;                  //! system state sensitivities
     std::vector<double*> hB_;                  //! system input sensitivities
     std::vector<double*> hb_;                  //! system offset term
-    Eigen::Matrix<double, state_dim, 1> hb0_;  //! intermediate container for intuitive transcription of first stage
+    typename MANIFOLD::Tangent hb0_;  //! intermediate container for intuitive transcription of first stage
 
     std::vector<double*> hQ_;                    //! pure state penalty hessian
     std::vector<double*> hS_;                    //! state-control cross-terms
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
index 87f06b734..4ae700686 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
@@ -18,20 +18,22 @@ namespace optcon {
  *
  * \todo uncouple from NLOptConSettings
  */
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+template <typename MANIFOLD, size_t CONTROL_DIM>
 class LQOCSolver
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+    static constexpr size_t STATE_DIM = MANIFOLD::TangentDim;
+    using SCALAR = typename MANIFOLD::Scalar;
+
+    typedef LQOCProblem<MANIFOLD, CONTROL_DIM> 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) {}
-
     virtual ~LQOCSolver() = default;
 
     /*!
@@ -49,20 +51,20 @@ class LQOCSolver
 
     //! setup and configure the box constraints
     // return true if configuration changed, otherwise false
-    virtual bool configureInputBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+    virtual bool configureInputBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
     {
         throw std::runtime_error("input box constraints are not available for this solver.");
     }
 
     // return true if configuration changed, otherwise false
-    virtual bool configureStateBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+    virtual bool configureStateBoxConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
     {
         throw std::runtime_error("state box constraints are not available for this solver.");
     }
 
     //! setup and configure the general (in)equality constraints
     // return true if configuration changed, otherwise false
-    virtual bool configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+    virtual bool configureGeneralConstraints(std::shared_ptr<LQOCProblem_t> lqocProblem)
     {
         throw std::runtime_error("general constraints are not available for this solver.");
     }
@@ -81,19 +83,16 @@ class LQOCSolver
     //! extract the solution (can be overriden if additional extraction steps required in specific solver)
     virtual void computeStatesAndControls() = 0;
     //! return solution for state
-    const ct::core::StateVectorArray<STATE_DIM, SCALAR>& getSolutionState() { return x_sol_; }
+    const core::DiscreteArray<typename MANIFOLD::Tangent>& getSolutionState() { return x_sol_; }
     //! return solution for control
     const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& getSolutionControl() { return u_sol_; }
-
     //! return TVLQR feedback matrices
     virtual void computeFeedbackMatrices() = 0;
     const ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& getSolutionFeedback() { return L_; }
-
     //! compute iLQR-style lv
     virtual void compute_lv() = 0;
     //! return iLQR-style feedforward lv
     virtual const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& get_lv() { return lv_; }
-
     //! return the smallest eigenvalue
     virtual SCALAR getSmallestEigenvalue()
     {
@@ -106,7 +105,7 @@ class LQOCSolver
 
     std::shared_ptr<LQOCProblem_t> lqocProblem_;
 
-    core::StateVectorArray<STATE_DIM, SCALAR> x_sol_;            // solution in x
+    core::DiscreteArray<typename MANIFOLD::Tangent> x_sol_;            // solution in x
     core::ControlVectorArray<CONTROL_DIM, SCALAR> u_sol_;        // solution in u
     ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> L_;  // solution feedback
     ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> lv_;       // feedforward increment (iLQR-style)
diff --git a/ct_optcon/test/CMakeLists.txt b/ct_optcon/test/CMakeLists.txt
index 44ae14c93..49c1c189a 100644
--- a/ct_optcon/test/CMakeLists.txt
+++ b/ct_optcon/test/CMakeLists.txt
@@ -45,14 +45,19 @@ if(BUILD_TESTS)
         message(STATUS "ct_optcon: building unit tests requiring CPPADCG")
     #    package_add_test(constraint_comparison constraint/ConstraintComparison.cpp)
     #    package_add_test(constraint_test constraint/ConstraintTest.cpp)
-        package_add_test(CostFunctionTests costfunction/CostFunctionTests.cpp)
-        package_add_test(LoadFromFileTest costfunction/LoadFromFileTest.cpp)
+    #    package_add_test(CostFunctionTests costfunction/CostFunctionTests.cpp)
+    #    package_add_test(LoadFromFileTest costfunction/LoadFromFileTest.cpp)
     endif()
     
     #package_add_test(dms_test dms/oscillator/oscDMSTest.cpp)
     #package_add_test(dms_test_all_var dms/oscillator/oscDMSTestAllVariants.cpp)
-    package_add_test(system_interface_test system_interface/SystemInterfaceTest.cpp)
+    #package_add_test(system_interface_test system_interface/SystemInterfaceTest.cpp)
     
+    add_executable(ManifoldLQOCTest solver/linear/ManifoldLQOCTest.cpp)
+    target_link_libraries(ManifoldLQOCTest ct_optcon)
+    set_target_properties(ManifoldLQOCTest PROPERTIES FOLDER test)
+    list(APPEND UNIT_TEST_TARGETS ManifoldLQOCTest)
+
     if(HPIPM)
         message(STATUS "ct_optcon: building unit tests requiring HPIPM")
         ## some legacy executables (TODO: make example or make test)
diff --git a/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
index 685d5a746..a98b4d5e3 100644
--- a/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
+++ b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
@@ -17,11 +17,12 @@ TEST(HPIPMInterfaceTest, compareSolvers)
 {
     const size_t state_dim = 8;
     const size_t control_dim = 3;
+    using State = ct::core::EuclideanState<state_dim>;
 
     int N = 5;
     double dt = 0.5;
 
-    typedef ct::optcon::LQOCProblem<state_dim, control_dim> LQOCProblem_t;
+    typedef ct::optcon::LQOCProblem<State, control_dim> LQOCProblem_t;
     std::shared_ptr<LQOCProblem_t> lqocProblem_hpipm(new LQOCProblem_t(N));
     std::shared_ptr<LQOCProblem_t> lqocProblem_gnriccati(new LQOCProblem_t(N));
 
@@ -46,12 +47,11 @@ TEST(HPIPMInterfaceTest, compareSolvers)
     R *= 2 * 2.0;
 
     // create a cost function
-    ct::optcon::CostFunctionQuadraticSimple<state_dim, control_dim> costFunction(
-        Q, R, -stateOffset, u0, -stateOffset, Q);
+    ct::optcon::CostFunctionQuadraticSimple<State, control_dim> costFunction(Q, R, -stateOffset, u0, -stateOffset, Q);
 
     // create a continuous-time example system and discretize it
-    std::shared_ptr<ct::core::LinearSystem<state_dim, control_dim>> system(new LinkedMasses());
-    ct::core::SensitivityApproximation<state_dim, control_dim> discretizedSystem(
+    std::shared_ptr<ct::core::LinearSystem<State, control_dim, ct::core::CONTINUOUS_TIME>> system(new LinkedMasses());
+    ct::core::SensitivityApproximation<State, control_dim> discretizedSystem(
         dt, system, ct::optcon::NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
 
     // initialize the linear quadratic optimal control problems
@@ -60,7 +60,7 @@ TEST(HPIPMInterfaceTest, compareSolvers)
 
 
     // create hpipm solver instance, set and solve problem
-    ct::optcon::HPIPMInterface<state_dim, control_dim> hpipm;
+    ct::optcon::HPIPMInterface<State, control_dim> hpipm;
     hpipm.setProblem(lqocProblem_hpipm);
     hpipm.solve();
     hpipm.computeStatesAndControls();
@@ -68,7 +68,7 @@ TEST(HPIPMInterfaceTest, compareSolvers)
     hpipm.compute_lv();
 
     // create GNRiccati solver instance, set and solve problem
-    ct::optcon::GNRiccatiSolver<state_dim, control_dim> gnriccati;
+    ct::optcon::GNRiccatiSolver<State, control_dim> gnriccati;
     gnriccati.setProblem(lqocProblem_gnriccati);
     gnriccati.solve();
     gnriccati.computeStatesAndControls();
@@ -76,8 +76,8 @@ TEST(HPIPMInterfaceTest, compareSolvers)
     gnriccati.compute_lv();
 
     // compute and retrieve solutions
-    ct::core::StateVectorArray<state_dim> x_sol_hpipm = hpipm.getSolutionState();
-    ct::core::StateVectorArray<state_dim> x_sol_gnrccati = gnriccati.getSolutionState();
+    ct::core::DiscreteArray<State> x_sol_hpipm = hpipm.getSolutionState();
+    ct::core::DiscreteArray<State> x_sol_gnrccati = gnriccati.getSolutionState();
     ct::core::ControlVectorArray<control_dim> u_sol_hpipm = hpipm.getSolutionControl();
     ct::core::ControlVectorArray<control_dim> u_sol_gnrccati = gnriccati.getSolutionControl();
     ct::core::FeedbackArray<state_dim, control_dim> K_sol_hpipm = hpipm.getSolutionFeedback();
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTest.h b/ct_optcon/test/solver/linear/LQOCSolverTest.h
index 57d204d8f..d97f89231 100644
--- a/ct_optcon/test/solver/linear/LQOCSolverTest.h
+++ b/ct_optcon/test/solver/linear/LQOCSolverTest.h
@@ -17,45 +17,46 @@ TEST(LQOCSolverTest, compareHPIPMandRiccati)
     const size_t N = 5;
     const double dt = 0.5;
 
+    using State = core::EuclideanState<state_dim>;
+
     bool verbose = false;  // optional verbose output
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
-    std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>);
-    std::shared_ptr<LQOCSolver<state_dim, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<state_dim, control_dim>);
+    std::shared_ptr<LQOCSolver<State, control_dim>> hpipmSolver(new HPIPMInterface<State, control_dim>);
+    std::shared_ptr<LQOCSolver<State, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<State, control_dim>);
 
     // store them, and identifying names, in a vectors
-    std::vector<std::shared_ptr<LQOCSolver<state_dim, control_dim>>> lqocSolvers;
+    std::vector<std::shared_ptr<LQOCSolver<State, control_dim>>> lqocSolvers;
     lqocSolvers.push_back(gnRiccatiSolver);
     lqocSolvers.push_back(hpipmSolver);
     std::vector<std::string> solverNames = {"Riccati", "HPIPM"};
 
     // create linear-quadratic optimal control problem containers
-    std::vector<std::shared_ptr<LQOCProblem<state_dim, control_dim>>> problems;
-    std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
-    std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem2(new LQOCProblem<state_dim, control_dim>(N));
+    std::vector<std::shared_ptr<LQOCProblem<State, control_dim>>> problems;
+    std::shared_ptr<LQOCProblem<State, control_dim>> lqocProblem1(new LQOCProblem<State, control_dim>(N));
+    std::shared_ptr<LQOCProblem<State, control_dim>> lqocProblem2(new LQOCProblem<State, control_dim>(N));
 
     problems.push_back(lqocProblem1);
     problems.push_back(lqocProblem2);
 
     // create a continuous-time example system and discretize it
-    std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(new example::SpringLoadedMassLinear());
-    core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+    std::shared_ptr<core::LinearSystem<State, control_dim, core::CONTINUOUS_TIME>> exampleSystem(
+        new example::SpringLoadedMassLinear());
+    core::SensitivityApproximation<State, control_dim> discreteExampleSystem(
         dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
 
     // nominal control
     ct::core::ControlVector<control_dim> u0;
     u0.setZero();  // by definition
-    // initial state
-    ct::core::StateVector<state_dim> x0;
+    // initial state and desired final state
+    State x0, xf;
     x0.setZero();  // by definition
-    // desired final state
-    ct::core::StateVector<state_dim> xf;
     xf << -1, 0;
 
     // create pointer to a cost function
     auto costFunction = example::createSpringLoadedMassCostFunction(xf);
 
-    ct::core::StateVector<state_dim> b;
+    State b;
     b << 0.1, 0.1;
 
     // initialize the optimal control problems for both solvers
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 01918bc24..7efcd6535 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -1,5 +1,7 @@
 
 #include <ct/optcon/optcon.h>
+#include <ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp>
+#include <ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp>
 
 using namespace ct::core;
 using namespace ct::optcon;
@@ -11,25 +13,27 @@ using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
 const size_t state_dim = ManifoldState_t::TangentDim;
 const size_t control_dim = 3;
 
-class DiscrSO3LTITestSystem final : public LTISystem<ManifoldState_t, control_dim, DISCRETE_TIME>
+class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    DiscrSO3LTITestSystem(const Eigen::Matrix<double, state_dim, state_dim> A_test,
-        const Eigen::Matrix<double, control_dim, control_dim> B_test,
-        const ManifoldState_t& m_ref = ManifoldState_t::NeutralElement())
-        : m_ref_(m_ref)
+    DiscrSO3LTITestSystem() {}
+    virtual void computeControlledDynamics(const ManifoldState_t& m,
+        const Time_t& n,
+        const ct::core::ControlVector<control_dim>& u,
+        ManifoldState_t::Tangent& dx) override
     {
-        this->A() << A_test;
-        this->B() << B_test;
+        dx = u;
     }
 
+    virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(); }
     /**
      * @brief the log operator is defined as expressing the tangent vector w.r.t. m_ref_
      */
     virtual ManifoldState_t::Tangent lift(const ManifoldState_t& m) override
     {
+        //throw std::runtime_error("not impl");
         return ManifoldState_t::Tangent::Zero();  // the system stays where it is (combination with A = identity())
         /*return m.rminus(m);*/
     }
@@ -37,29 +41,66 @@ class DiscrSO3LTITestSystem final : public LTISystem<ManifoldState_t, control_di
     //     throw std::runtime_error("not implemetned.");
     //     /*return m_ref_.rplus(t);*/ }
 protected:
-    ManifoldState_t m_ref_;
+    //ManifoldState_t m_ref_;
 };
 
 
+// TODO: make this a unit test
+void testParallelTransport()
+{
+    auto origin = manif::SE2<double>(0, 0, 0);
+    Eigen::Matrix3d b;
+    b.setIdentity();  // some basis, assumed to be expressed in the manifolds over which we iterate
+
+    for (double theta = M_PI / 2; theta <= 2 * M_PI; theta += M_PI / 2)
+    {
+        std::cout << "case theta = " << theta << std::endl;
+        auto m = manif::SE2<double>(0, 0, theta);
+        Eigen::Matrix3d Jl, Jr;  // jacobians
+        auto d_rminus = m.rminus(origin, Jl, Jr);
+        auto adj_rminus = d_rminus.exp().adj();  // the adjoint for transportation to the origin
+
+        auto d_between = m.between(origin);
+        auto adj_between = d_rminus.exp().adj();
+
+        // ASSERT_TRUE(adj_rminus.isApprox(adj_between));
+
+        std::cout << adj_rminus * b << std::endl << std::endl;   // express basis b in origin-manifold
+        std::cout << adj_between * b << std::endl << std::endl;  // express basis b in origin-manifold
+        // std::cout << Jr * Jl * b << std::endl << std::endl;  // TODO: how does the Jac relate to the adjoint here?
+        std::cout << std::endl;
+    }
+
+    // auto m_pi_2 = manif::SE2<double>(0, 0, M_PI / 2);
+    // auto m_pi = manif::SE2<double>(0, 0, M_PI);
+    // auto m_mpi_2 = manif::SE2<double>(0, 0, -M_PI / 2);
+    //
+    //
+    // std::cout << m_0.adj() * b << std::endl << std::endl;
+    // std::cout << m_pi_2.adj() * b << std::endl << std::endl;
+    // std::cout << m_pi.adj() * b << std::endl << std::endl;
+    // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
+}
+
 int main(int argc, char** argv)
 {
     std::cout << std::fixed;
 
-    const size_t N = 5;
-    const double dt = 0.25;
+    const size_t N = 20*3;
+    const double dt = 0.125 / 3.0;
 
-    const ManifoldState_t x0 = manif::SO3<double>(0, 0, 0);
+    const ManifoldState_t x0 = manif::SO3<double>(M_PI, 0, M_PI);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
-    ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj_init(N);  // init control traj
+    ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
     for (size_t i = 0; i < N; i++)
-        u_traj_init[i] = ct::core::ControlVector<control_dim>::Zero() * dt;
+        u_traj[i] = ct::core::ControlVector<control_dim>::Ones() * dt;
 
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
     std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
         new HPIPMInterface<ManifoldState_t, control_dim>);
     std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
-        new GNRiccatiSolver<ManifoldState_t, control_dim>);
+        new AugGNRiccatiSolver<ManifoldState_t, control_dim>);
 
     // store them, and identifying names, in a vectors
     std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
@@ -78,22 +119,20 @@ int main(int argc, char** argv)
     problems.push_back(lqocProblem2);
 
     // create a discrete-time manifold system
-    Eigen::Matrix<double, state_dim, state_dim> A = Eigen::Matrix<double, state_dim, state_dim>::Zero();
-    Eigen::Matrix<double, control_dim, control_dim> B;
-    A << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0;  // a slightly unstable system
-    B << 1, 0, 0, 0, 1, 0, 0, 0, 1;        // direct action
-    std::shared_ptr<LinearSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
-        new DiscrSO3LTITestSystem(A, B, x0));
+    std::shared_ptr<ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
+        new DiscrSO3LTITestSystem());
+    std::shared_ptr<ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>> linearizer(
+        new ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>(exampleSystem));
 
 
     // create a cost function
     Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
     Eigen::Matrix<double, control_dim, control_dim> R;
-    Q_final << 100, 0, 0, 0, 100, 0, 0, 0, 100;
+    Q_final << 10000, 0, 0, 0, 10000, 0, 0, 0, 10000;
     //Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;
     Q.setZero();
     R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
-    ManifoldState_t x_final = manif::SO3<double>(M_PI / 2, 0, 0);
+    ManifoldState_t x_final = manif::SO3<double>(0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
     ManifoldState_t x_nominal = x0;
     ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
@@ -101,7 +140,7 @@ int main(int argc, char** argv)
         new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
 
     ManifoldState_t::Tangent b;
-    b.setZero();  // todo why this?
+    b.setZero();  // TODO why this?
 
     // integrate an initial state with the open-loop system to get initial trajectories
     ManifoldState_t x_curr;
@@ -109,121 +148,125 @@ int main(int argc, char** argv)
     x_curr = x0;
     x_traj.front() = x0;
     std::cout << "integrate an random initial state with the unstable system" << std::endl;
-    std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     for (size_t i = 0; i < N; i++)
     {
-        exampleSystem->computeControlledDynamics(x_curr, 0, u_traj_init[i], dx);
+        exampleSystem->computeControlledDynamics(x_curr, 0, u_traj[i], dx);
         x_curr = x_curr + dx;
         x_traj[i + 1] = x_curr;
-        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
-
-    // initialize the optimal control problems for both solvers
-    problems[0]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj_init, *exampleSystem, *costFunction, b, dt);
-    problems[1]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj_init, *exampleSystem, *costFunction, b, dt);
+    size_t nIter = 6;
+    for (size_t iter = 0; iter < nIter; iter++)
+    {
+        // initialize the optimal control problems for both solvers
+        problems[0]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
+        problems[1]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
 
 
-    // HACKY corrections // TODO: move somewhere meaningful
-    for (size_t idx : {0, 1})  // for both solvers
-    {
-        // intermediate stages cost transportation
-        for (size_t i = 0; i < N; i++)
+        // HACKY corrections // TODO: move somewhere meaningful
+        for (size_t idx : {0})  // for riccati solver only
         {
-            Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
-            auto e = x_nominal.rminus(x_traj[i], Jl, Jr);  // compute PT matrix w.r.t. current ref traj
+            // intermediate stages cost transportation
+            for (size_t i = 0; i < N; i++)
+            {
+                auto e = x_nominal.rminus(x_traj[i]);
+                // compute PT matrix w.r.t. current ref traj // TODO: clarifiy formulation of error in cost function
+                auto e_adj = (e.exp()).adj();
+                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] * e_adj.transpose();
+                problems[idx]->qv_[i] = e_adj * problems[idx]->qv_[i];
+            }
+            // terminal stage transportation
+            // TODO: clarifiy formulation of error in cost function
+            auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.t. current ref traj
             auto e_adj = (e.exp()).adj();
-            problems[idx]->Q_[i] = e_adj.transpose() * problems[idx]->Q_[i] * e_adj;
-            problems[idx]->qv_[i] = e_adj.transpose() * problems[idx]->qv_[i];
-        }
-        // terminal stage transportation
-        Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
-        auto e = x_final.rminus(x_traj[N], Jl, Jr);  // compute PT matrix w.r.t. current ref traj
-        auto e_adj = (e.exp()).adj();
-        std::cout << "cost adj" << std::endl << e_adj << std::endl;
-        std::cout << "cost Jr" << std::endl << Jr << std::endl;
-        problems[idx]->Q_.back() = e_adj.transpose() * problems[idx]->Q_.back() * e_adj;
-        problems[idx]->qv_.back() = e_adj.transpose() * problems[idx]->qv_.back();
-
-        // dynamics transportation
-        for (size_t i = 0; i < N; i++)
-        {
-            // std::cout << "dyn transport matrices" << std::endl;
-            Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
-
-            auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);  // TODO: is it the right way round?
-            auto l_adj = (l.exp()).adj();
-            //std::cout << "Jl" << std::endl << Jl << std::endl;
-            //std::cout << "Jr" << std::endl << Jr << std::endl;
-            //std::cout << "l_adj" << std::endl << l_adj << std::endl;
-            //std::cout << std::endl;
-            problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];  // todo: are those the right
-            problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];  // TODO: are those the right
+            //std::cout << "cost adj" << std::endl << e_adj << std::endl;
+            problems[idx]->Q_.back()  = /*e_adj * */  problems[idx]->Q_.back(); // * e_adj.transpose();
+            problems[idx]->qv_.back() = /*e_adj * */  problems[idx]->qv_.back();
+
+            // dynamics transportation
+            for (size_t i = 0; i < N; i++)
+            {
+                // std::cout << "dyn transport matrices" << std::endl;
+                Eigen::Matrix3d Jl, Jr;
+                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);  // TODO: is it the right way round?
+                auto l_adj = (l.exp()).adj();
+                //std::cout << "l_adj" << std::endl << l_adj << std::endl;
+                //std::cout << std::endl;
+                // problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];  // todo: are those the right
+                // problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];  // TODO: are those the right
+                problems[idx]->Acal_[i + 1] = l_adj;
+            }
+
+
+            // set the problem pointers
+            lqocSolvers[idx]->setProblem(problems[idx]);
+
+            // allocate memory (if required)
+            lqocSolvers[idx]->initializeAndAllocate();
+
+            // solve the problems...
+            lqocSolvers[idx]->solve();
+
+            // postprocess data
+            lqocSolvers[idx]->computeStatesAndControls();
+            lqocSolvers[idx]->computeFeedbackMatrices();
+            lqocSolvers[idx]->compute_lv();
         }
 
-
-        // set the problem pointers
-        lqocSolvers[idx]->setProblem(problems[idx]);
-
-        // allocate memory (if required)
-        lqocSolvers[idx]->initializeAndAllocate();
-
-        // solve the problems...
-        lqocSolvers[idx]->solve();
-
-        // postprocess data
-        lqocSolvers[idx]->computeStatesAndControls();
-        lqocSolvers[idx]->computeFeedbackMatrices();
-        lqocSolvers[idx]->compute_lv();
-    }
-
-    // retrieve solutions from both solvers
-    auto xSol_riccati = lqocSolvers[0]->getSolutionState();
-    auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
-    ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
-    ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
-    auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
-    auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
-    ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = lqocSolvers[1]->getSolutionFeedback();
-    ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = lqocSolvers[1]->get_lv();
-
-    std::cout << std::endl << std::endl;
-    std::cout << std::setprecision(3) << "Forward integrated closed-loop solution:" << std::endl;
-    x_curr = x0;
-    std::cout << "m init: " << x_curr << std::endl;
-    for (size_t i = 0; i < N; i++)
-    {
-        Eigen::Matrix<double, state_dim, state_dim> j1, j2;
-        auto x_err = x_curr.rminus(x_traj[i], j1, j2);
-        // std::cout << "lv_sol_riccati[i]: " << lv_sol_riccati[i].transpose() << std::endl;
-        // std::cout << "uSol_riccati[i]: " << uSol_riccati[i].transpose() << std::endl;
-        // std::cout << "KSol_riccati: " << std::endl << KSol_riccati[i] << std::endl;
-        // TODO: some term is missing here;
-        auto u = u_traj_init[i] + lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
-        exampleSystem->computeControlledDynamics(x_curr, i * dt, u, dx);
-        Eigen::Quaterniond old_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
-        x_curr = x_curr + dx;
-        Eigen::Quaterniond new_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
-        std::cout << "m: " << x_curr << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
-    }
-    std::cout << std::endl << std::endl;
-
-    std::cout << std::setprecision(3) << "dx solution from riccati solver and directly added state_traj" << std::endl;
-    ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
-    for (size_t j = 0; j < xSol_riccati.size(); j++)
-    {
-        x_solver_direct[j] = x_traj[j] + xSol_riccati[j];
-        double angularDiff = 0;
-        if (j > 0)
+        // retrieve solutions from both solvers
+        auto xSol_riccati = lqocSolvers[0]->getSolutionState();
+        auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
+        auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
+        auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = lqocSolvers[1]->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = lqocSolvers[1]->get_lv();
+
+        std::cout << std::endl << std::endl;
+        //std::cout << std::setprecision(4) << "dx solution from riccati solver and directly added state_traj, iter "
+        //          << iter << std::endl;
+        //ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
+        //for (size_t j = 0; j < xSol_riccati.size(); j++)
+        //{
+        //    x_solver_direct[j] = x_traj[j] + xSol_riccati[j];
+        //    double angularDiff = 0;
+        //    if (j > 0)
+        //    {
+        //        Eigen::Quaterniond old_rot(x_solver_direct[j - 1].w(), x_solver_direct[j - 1].x(),
+        //            x_solver_direct[j - 1].y(), x_solver_direct[j - 1].z());
+        //        Eigen::Quaterniond new_rot(
+        //            x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
+        //        angularDiff = old_rot.angularDistance(new_rot);
+        //    }
+        //    std::cout << "m:" << x_solver_direct[j] << "\t dx:" << xSol_riccati[j].transpose()
+        //              << "\t -- rot diff norm(): " << angularDiff << std::endl;
+        //}
+
+        std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
+        x_curr = x0;
+        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
+        for (size_t i = 0; i < N; i++)
         {
-            Eigen::Quaterniond old_rot(x_solver_direct[j - 1].w(), x_solver_direct[j - 1].x(),
-                x_solver_direct[j - 1].y(), x_solver_direct[j - 1].z());
-            Eigen::Quaterniond new_rot(
-                x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
-            angularDiff = old_rot.angularDistance(new_rot);
+            ManifoldState_t::Tangent x_err = x_curr.rminus(x_traj_prev[i]);
+            // TODO: some term is missing here;
+            const ct::core::ControlVector<control_dim> u =
+                u_traj[i] + lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
+            u_traj[i] = u;
+
+            exampleSystem->computeControlledDynamics(x_curr, i * dt, u, dx);
+            Eigen::Quaterniond old_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
+            x_curr = x_curr + dx;
+            Eigen::Quaterniond new_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
+            std::cout << "m: " << x_curr << "\t tan: " << x_curr.log()
+                      << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
+
+            x_traj[i + 1] = x_curr;
         }
-        std::cout << "\t m:" << x_solver_direct[j] << "\t dx:" << xSol_riccati[j].transpose()
-                  << "\t -- rot diff norm(): " << angularDiff << std::endl;
-    }
+        std::cout << std::endl << std::endl;
+    }  // end iter
     return 1;
 }
\ No newline at end of file
diff --git a/ct_optcon/test/testSystems/LinkedMasses.h b/ct_optcon/test/testSystems/LinkedMasses.h
index 452c0bd73..ee366ebb1 100644
--- a/ct_optcon/test/testSystems/LinkedMasses.h
+++ b/ct_optcon/test/testSystems/LinkedMasses.h
@@ -26,7 +26,7 @@ void dmcopy(int row, int col, double* A, int lda, double* B, int ldb)
     }
 }
 
-class LinkedMasses : public LinearSystem<8, 3>
+class LinkedMasses : public LinearSystem<EuclideanState<8>, 3, CONTINUOUS_TIME>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -35,6 +35,8 @@ class LinkedMasses : public LinearSystem<8, 3>
     static const int control_dim = 3;
     static const int pp = state_dim / 2;  // number of masses
 
+    using State = EuclideanState<state_dim>;
+
     LinkedMasses()
     {
         A_.setZero();
@@ -76,14 +78,14 @@ class LinkedMasses : public LinearSystem<8, 3>
     }
 
 
-    const state_matrix_t& getDerivativeState(const StateVector<state_dim>& x,
+    const state_matrix_t& getDerivativeState(const State& x,
         const ControlVector<control_dim>& u,
         const double t = 0.0) override
     {
         return A_;
     }
 
-    const state_control_matrix_t& getDerivativeControl(const StateVector<state_dim>& x,
+    const state_control_matrix_t& getDerivativeControl(const State& x,
         const ControlVector<control_dim>& u,
         const double t = 0.0) override
     {
@@ -97,11 +99,12 @@ class LinkedMasses : public LinearSystem<8, 3>
 };
 
 
-class LinkedMasses2 : public ControlledSystem<8, 3>
+class LinkedMasses2 : public ControlledSystem<EuclideanState<8>, 3, CONTINUOUS_TIME>
 {
 public:
     static const int state_dim = 8;
     static const int control_dim = 3;
+    using State = EuclideanState<state_dim>;
 
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -150,10 +153,10 @@ class LinkedMasses2 : public ControlledSystem<8, 3>
     }
 
     LinkedMasses2* clone() const override { return new LinkedMasses2(); };
-    void computeControlledDynamics(const ct::core::StateVector<state_dim>& state,
+    void computeControlledDynamics(const State& state,
         const double& t,
         const ct::core::ControlVector<control_dim>& control,
-        ct::core::StateVector<state_dim>& derivative) override
+        State::Tangent& derivative) override
     {
         derivative = A_ * state + B_ * control + b_;
     }
diff --git a/ct_optcon/test/testSystems/SpringLoadedMass.h b/ct_optcon/test/testSystems/SpringLoadedMass.h
index b85a8a41e..3c9335325 100644
--- a/ct_optcon/test/testSystems/SpringLoadedMass.h
+++ b/ct_optcon/test/testSystems/SpringLoadedMass.h
@@ -11,17 +11,23 @@ namespace example {
 
 
 //! Dynamics class for the GNMS unit test
-class SpringLoadedMass : public core::ControlledSystem<2, 1>
+class SpringLoadedMass : public core::ControlledSystem<core::EuclideanState<2>, 1, core::CONTINUOUS_TIME>
 {
 public:
     static const size_t state_dim = 2;    // position, velocity
     static const size_t control_dim = 1;  // force
 
-    SpringLoadedMass() : core::ControlledSystem<state_dim, control_dim>(core::SYSTEM_TYPE::SECOND_ORDER) {}
-    void computeControlledDynamics(const core::StateVector<state_dim>& state,
-        const core::Time& t,
+    SpringLoadedMass()
+        : core::ControlledSystem<core::EuclideanState<state_dim>, control_dim, core::CONTINUOUS_TIME>(
+              core::SYSTEM_TYPE::SECOND_ORDER)
+    {
+    }
+
+    // compute dynamics
+    void computeControlledDynamics(const core::EuclideanState<state_dim>& state,
+        const double& t,
         const core::ControlVector<control_dim>& control,
-        core::StateVector<state_dim>& derivative) override
+        core::EuclideanState<state_dim>::Tangent& derivative) override
     {
         derivative(0) = state(1);
         derivative(1) = control(0) - kStiffness * state(0) + 0.1;  // mass is 1 kg
@@ -32,7 +38,7 @@ class SpringLoadedMass : public core::ControlledSystem<2, 1>
 };
 
 //! Linear system class for the GNMS unit test
-class SpringLoadedMassLinear : public core::LinearSystem<2, 1>
+class SpringLoadedMassLinear : public core::LinearSystem<core::EuclideanState<2>, 1, core::CONTINUOUS_TIME>
 {
 public:
     static const size_t state_dim = 2;    // position, velocity
@@ -44,7 +50,7 @@ class SpringLoadedMassLinear : public core::LinearSystem<2, 1>
     state_control_matrix_t B_;
 
 
-    const state_matrix_t& getDerivativeState(const core::StateVector<state_dim>& x,
+    const state_matrix_t& getDerivativeState(const core::EuclideanState<state_dim>& x,
         const core::ControlVector<control_dim>& u,
         const double t = 0.0) override
     {
@@ -52,7 +58,7 @@ class SpringLoadedMassLinear : public core::LinearSystem<2, 1>
         return A_;
     }
 
-    const state_control_matrix_t& getDerivativeControl(const core::StateVector<state_dim>& x,
+    const state_control_matrix_t& getDerivativeControl(const core::EuclideanState<state_dim>& x,
         const core::ControlVector<control_dim>& u,
         const double t = 0.0) override
     {
@@ -64,7 +70,8 @@ class SpringLoadedMassLinear : public core::LinearSystem<2, 1>
 };
 
 
-std::shared_ptr<CostFunctionQuadratic<2, 1>> createSpringLoadedMassCostFunction(const core::StateVector<2>& x_final)
+std::shared_ptr<CostFunctionQuadratic<core::EuclideanState<2>, 1>> createSpringLoadedMassCostFunction(
+    const core::EuclideanState<2>& x_final)
 {
     Eigen::Matrix<double, 2, 2> Q;
     Q << 1.0, 0, 0, 1.0;
@@ -79,11 +86,12 @@ std::shared_ptr<CostFunctionQuadratic<2, 1>> createSpringLoadedMassCostFunction(
     Eigen::Matrix<double, 2, 2> Q_final;
     Q_final << 10.0, 0, 0, 10.0;
 
-    std::shared_ptr<CostFunctionQuadratic<2, 1>> quadraticCostFunction(
-        new CostFunctionQuadraticSimple<2, 1>(Q, R, x_nominal, u_nominal, x_final, Q_final));
+    std::shared_ptr<CostFunctionQuadratic<core::EuclideanState<2>, 1>> quadraticCostFunction(
+        new CostFunctionQuadraticSimple<core::EuclideanState<2>, 1>(Q, R, x_nominal, u_nominal, x_final, Q_final));
 
     return quadraticCostFunction;
 }
-}
-}
-}
+
+}  // namespace example
+}  // namespace optcon
+}  // namespace ct

From 2f7a11684fc07c1bb322938adb5c7d381d290fe2 Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Sun, 26 Apr 2020 22:02:06 +0200
Subject: [PATCH 03/21] working ex

---
 .../ct/optcon/problem/LQOCProblem-impl.hpp    |  15 +-
 .../include/ct/optcon/problem/LQOCProblem.hpp |   9 +-
 .../solver/lqp/AugGNRiccatiSolver-impl.hpp    |  58 +++++---
 .../optcon/solver/lqp/AugGNRiccatiSolver.hpp  |   6 +-
 .../solver/lqp/GNRiccatiSolver-impl.hpp       |   5 +-
 .../ct/optcon/solver/lqp/GNRiccatiSolver.hpp  |   1 +
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 140 +++++++++++++-----
 7 files changed, 162 insertions(+), 72 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
index c9bb43b73..1178bfde2 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -70,7 +70,7 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::changeNumStages(int N)
     q_.resize(N + 1);
     qv_.resize(N + 1);
     Q_.resize(N + 1);
-    Acal_.resize(N+1);
+    Adj_x_.resize(N + 1);
 
     rv_.resize(N);
     R_.resize(N);
@@ -102,7 +102,7 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setZero(const int& nGenConstr)
     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());
-    Acal_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Identity());
+    Adj_x_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Identity());
     rv_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
     R_.setConstant(core::ControlMatrix<CONTROL_DIM, SCALAR>::Zero());
     q_.setConstant((SCALAR)0.0);
@@ -277,7 +277,7 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProb
     const core::ControlVector<CONTROL_DIM, SCALAR>& u0,
     ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
     ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
-    const typename MANIFOLD::Tangent& stateOffset,
+    const typename MANIFOLD::Tangent& b,
     const double dt)
 {
     setZero();
@@ -288,7 +288,7 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProb
 
     A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
     B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
-    b_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, stateOffset);
+    b_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, b);
 
     // feed current state and control to cost function
     costFunction.setCurrentStateAndControl(x0, u0, 0);
@@ -314,18 +314,21 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProb
     const ct::core::DiscreteArray<core::ControlVector<CONTROL_DIM, SCALAR>>& u_traj,
     ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
     ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
-    const typename MANIFOLD::Tangent& stateOffset,
+    const ct::core::DiscreteArray<typename MANIFOLD::Tangent>& b,
     const double dt)
 {
     setZero();
 
+    if (b.size() != K_ + 1)
+        throw std::runtime_error("b size not correct.");
+
     // get LTI dynamics
     core::StateMatrix<STATE_DIM, SCALAR> A;
     core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
     linearSystem.getDerivatives(A, B, x_traj.front(), u_traj.front(), 0);
     A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
     B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
-    b_ = core::DiscreteArray<typename MANIFOLD::Tangent>(K_ + 1, stateOffset);
+    b_ = b;
 
     // eval costs around the reference trajectory
     for (int i = 0; i < K_; i++)
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
index df5407d33..ecfa757c3 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
@@ -204,7 +204,7 @@ class LQOCProblem
         const ct::core::DiscreteArray<core::ControlVector<CONTROL_DIM, SCALAR>>& u_traj,
         ct::core::LinearSystem<MANIFOLD, CONTROL_DIM, core::DISCRETE_TIME>& linearSystem,
         ct::optcon::CostFunctionQuadratic<MANIFOLD, CONTROL_DIM>& costFunction,
-        const typename MANIFOLD::Tangent& stateOffset,
+        const ct::core::DiscreteArray<typename MANIFOLD::Tangent>& stateOffset,
         const double dt);
 
     //! return a flag indicating whether this LQOC Problem is constrained or not
@@ -225,7 +225,12 @@ class LQOCProblem
     //! LQ approximation of the pure state penalty, including terminal state penalty
     ct::core::DiscreteArray<typename MANIFOLD::Tangent> qv_;
     ct::core::StateMatrixArray<STATE_DIM, SCALAR> Q_;
-    ct::core::StateMatrixArray<STATE_DIM, SCALAR> Acal_; // TODO: temp
+    /**
+     * @brief The adjoint array for parallel transporting to the preceding tangent space
+     * Adj_x_[k] holds the adjoint used for parallel transporting vectors in the tangent space at k to the tangent
+     * space at [k-1]. 
+     */
+    ct::core::StateMatrixArray<STATE_DIM, SCALAR> Adj_x_;
 
     //! LQ approximation of the pure control penalty
     ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> rv_;
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
index 39591d47b..f6d3ca33d 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
@@ -32,15 +32,15 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solve()
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int N)
+void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int n)
 {
-    if (N == this->lqocProblem_->getNumberOfStages() - 1)
+    if (n == this->lqocProblem_->getNumberOfStages() - 1)
         initializeCostToGo();
 
-    designController(N);
+    designController(n);
 
-    if (N > 0)
-        computeCostToGo(N);
+    if (n > 0)
+        computeCostToGo(n);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -63,7 +63,7 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];  // TODO: transport
+        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];
     }
 }
 
@@ -120,8 +120,8 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::changeNumberOfStages(int N)
 
     sv_.resize(N + 1);
     S_.resize(N + 1);
-    sv_tilda_.resize(N + 1);
-    S_tilda_.resize(N + 1);
+    sv_t_.resize(N + 1);
+    S_t_.resize(N + 1);
 
     N_ = N;
 }
@@ -139,8 +139,16 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeCostToGo()
     S_[N] = p.Q_[N];
     sv_[N] = p.qv_[N];
 
-    S_tilda_[N] = p.Acal_[N] * S_[N] * p.Acal_[N].transpose();
-    sv_tilda_[N] = p.Acal_[N] * sv_[N];
+    if (ct::core::is_euclidean<MANIFOLD>::value)  // euclidean case
+    {
+        S_t_[N] = S_[N];
+        sv_t_[N] = sv_[N];
+    }
+    else
+    {  // manifold case requires parallel transport of value function
+        S_t_[N] = p.Adj_x_[N] * S_[N] * p.Adj_x_[N].transpose();
+        sv_t_[N] = p.Adj_x_[N] * sv_[N];
+    }
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -149,20 +157,28 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
     LQOCProblem_t& p = *this->lqocProblem_;
 
     S_[k] = p.Q_[k];
-    S_[k].noalias() += p.A_[k].transpose() * S_tilda_[k + 1] * p.A_[k];
+    S_[k].noalias() += p.A_[k].transpose() * S_t_[k + 1] * p.A_[k];
     S_[k].noalias() -= this->L_[k].transpose() * Hi_[k] * this->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_tilda_[k + 1];
-    sv_[k].noalias() += p.A_[k].transpose() * S_tilda_[k + 1] * p.b_[k];
+    sv_[k].noalias() += p.A_[k].transpose() * sv_t_[k + 1];
+    sv_[k].noalias() += p.A_[k].transpose() * S_t_[k + 1] * p.b_[k];
     sv_[k].noalias() += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
     sv_[k].noalias() += this->L_[k].transpose() * gv_[k];
     sv_[k].noalias() += G_[k].transpose() * this->lv_[k];
 
-    S_tilda_[k] = p.Acal_[k] * S_[k] * p.Acal_[k].transpose();
-    sv_tilda_[k] = p.Acal_[k] * sv_[k];
+    if (ct::core::is_euclidean<MANIFOLD>::value)  // euclidean case
+    {
+        S_t_[k] = S_[k];
+        sv_t_[k] = sv_[k];
+    }
+    else
+    {  // manifold case requires parallel transport of value function to previous stage
+        S_t_[k] = p.Adj_x_[k] * S_[k] * p.Adj_x_[k].transpose();
+        sv_t_[k] = p.Adj_x_[k] * sv_[k];
+    }
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -171,16 +187,16 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
     LQOCProblem_t& p = *this->lqocProblem_;
 
     gv_[k] = p.rv_[k];
-    gv_[k].noalias() += p.B_[k].transpose() * sv_tilda_[k + 1];
-    //gv_[k].noalias() += p.B_[k].transpose() * S_tilda_[k + 1].template selfadjointView<Eigen::Lower>() * p.b_[k];
+    gv_[k].noalias() += p.B_[k].transpose() * sv_t_[k + 1];
+    gv_[k].noalias() += p.B_[k].transpose() * S_t_[k + 1].template selfadjointView<Eigen::Lower>() * p.b_[k];
 
-    G_[k] = p.P_[k];
+    G_[k] = p.P_[k];  // TODO: G_ could be left away for the first stage (efficiency)
     //G_[k].noalias() += B_[k].transpose() * S_[k+1] * A_[k];
-    G_[k].noalias() += p.B_[k].transpose() * S_tilda_[k + 1].template selfadjointView<Eigen::Lower>() * p.A_[k];
+    G_[k].noalias() += p.B_[k].transpose() * S_t_[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_tilda_[k + 1].template selfadjointView<Eigen::Lower>() * p.B_[k];
+    H_[k].noalias() += p.B_[k].transpose() * S_t_[k + 1].template selfadjointView<Eigen::Lower>() * p.B_[k];
 
     if (settings_.fixedHessianCorrection)
     {
@@ -223,7 +239,7 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
 
         Hi_inverse_[k] = -Hi_[k].template selfadjointView<Eigen::Lower>().llt().solve(ControlMatrix::Identity());
 
-        // calculate FB gain update
+        // calculate FB gain update // TODO: this could be left away for the first stage (efficiency)
         this->L_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * G_[k];
 
         // calculate FF update
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
index 846c251a5..2bf54c7ef 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
@@ -88,10 +88,12 @@ class AugGNRiccatiSolver : public LQOCSolver<MANIFOLD, CONTROL_DIM>
     ControlMatrixArray Hi_inverse_;
     ControlMatrix H_corrFix_;
 
+    //! terms for quadratic value function
     ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_;
     StateMatrixArray S_;
-    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_tilda_;
-    StateMatrixArray S_tilda_;
+    //! parallel-transported terms of value function
+    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_t_;
+    StateMatrixArray S_t_;
 
     int N_;
 
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 9f49c2d46..9a94d6a4b 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -63,7 +63,10 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];
+        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backwards pass"
+        StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backwards pass"
+        typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backwards pass"
+        this->x_sol_[k + 1] = A_orig * this->x_sol_[k] + B_orig * (this->u_sol_[k]) + b_orig;
     }
 }
 
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
index 7caf75f45..6d635fb31 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
@@ -33,6 +33,7 @@ class GNRiccatiSolver : public LQOCSolver<MANIFOLD, CONTROL_DIM>
     typedef typename Base::LQOCProblem_t LQOCProblem_t;
 
     typedef ct::core::StateMatrix<STATE_DIM, SCALAR> StateMatrix;
+    typedef ct::core::StateControlMatrix<STATE_DIM, control_dim, SCALAR> StateControlMatrix;
     typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> StateMatrixArray;
     typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> ControlVector;
     typedef ct::core::ControlMatrix<CONTROL_DIM, SCALAR> ControlMatrix;
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 7efcd6535..83fdeb736 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -86,27 +86,37 @@ int main(int argc, char** argv)
 {
     std::cout << std::fixed;
 
-    const size_t N = 20*3;
-    const double dt = 0.125 / 3.0;
+    const bool use_single_shooting = false;  // toggle between single and multiple shooting
+
+    const size_t N = 25;
+    const double dt = 0.05;
 
     const ManifoldState_t x0 = manif::SO3<double>(M_PI, 0, M_PI);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
+    ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
+        N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
     ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
     for (size_t i = 0; i < N; i++)
-        u_traj[i] = ct::core::ControlVector<control_dim>::Ones() * dt;
+        u_traj[i] = ct::core::ControlVector<control_dim>::Random() * dt;
 
+    // choose a random initial state
+    // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
+    for (size_t i = 1; i < N + 1; i++)
+    {
+        x_traj[i] = (ManifoldState_t::Random().log() * 0.2).exp();
+    }
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
-        new HPIPMInterface<ManifoldState_t, control_dim>);
     std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
+        new GNRiccatiSolver<ManifoldState_t, control_dim>);
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> augGnRiccatiSolver(
         new AugGNRiccatiSolver<ManifoldState_t, control_dim>);
 
     // store them, and identifying names, in a vectors
     std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
     lqocSolvers.push_back(gnRiccatiSolver);
-    lqocSolvers.push_back(hpipmSolver);
-    std::vector<std::string> solverNames = {"Riccati", "HPIPM"};
+    lqocSolvers.push_back(augGnRiccatiSolver);
+    std::vector<std::string> solverNames = {"Riccati", "AugRiccati"};
 
     // create linear-quadratic optimal control problem containers
     std::vector<std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>>> problems;
@@ -139,8 +149,6 @@ int main(int argc, char** argv)
     std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
         new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
 
-    ManifoldState_t::Tangent b;
-    b.setZero();  // TODO why this?
 
     // integrate an initial state with the open-loop system to get initial trajectories
     ManifoldState_t x_curr;
@@ -151,13 +159,16 @@ int main(int argc, char** argv)
     std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     for (size_t i = 0; i < N; i++)
     {
-        exampleSystem->computeControlledDynamics(x_curr, 0, u_traj[i], dx);
-        x_curr = x_curr + dx;
-        x_traj[i + 1] = x_curr;
+        exampleSystem->computeControlledDynamics(x_traj[i], 0, u_traj[i], dx);
+        x_curr = x_traj[i] + dx;
+        if (use_single_shooting)
+            x_traj[i + 1] = x_curr;
+        b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
+        std::cout << "b: " << b[i] << std::endl;
         std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
-    size_t nIter = 6;
+    size_t nIter = 13;
     for (size_t iter = 0; iter < nIter; iter++)
     {
         // initialize the optimal control problems for both solvers
@@ -166,7 +177,7 @@ int main(int argc, char** argv)
 
 
         // HACKY corrections // TODO: move somewhere meaningful
-        for (size_t idx : {0})  // for riccati solver only
+        for (size_t idx : {0, 1})
         {
             // intermediate stages cost transportation
             for (size_t i = 0; i < N; i++)
@@ -174,7 +185,8 @@ int main(int argc, char** argv)
                 auto e = x_nominal.rminus(x_traj[i]);
                 // compute PT matrix w.r.t. current ref traj // TODO: clarifiy formulation of error in cost function
                 auto e_adj = (e.exp()).adj();
-                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] * e_adj.transpose();
+                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] *
+                                       e_adj.transpose();  // TODO: sort out that thing with the cost function
                 problems[idx]->qv_[i] = e_adj * problems[idx]->qv_[i];
             }
             // terminal stage transportation
@@ -182,24 +194,28 @@ int main(int argc, char** argv)
             auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.t. current ref traj
             auto e_adj = (e.exp()).adj();
             //std::cout << "cost adj" << std::endl << e_adj << std::endl;
-            problems[idx]->Q_.back()  = /*e_adj * */  problems[idx]->Q_.back(); // * e_adj.transpose();
-            problems[idx]->qv_.back() = /*e_adj * */  problems[idx]->qv_.back();
+            problems[idx]->Q_.back() = /*e_adj * */ problems[idx]->Q_.back();  // * e_adj.transpose();
+            problems[idx]->qv_.back() = /*e_adj * */ problems[idx]->qv_.back();
 
             // dynamics transportation
             for (size_t i = 0; i < N; i++)
             {
                 // std::cout << "dyn transport matrices" << std::endl;
                 Eigen::Matrix3d Jl, Jr;
-                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);  // TODO: is it the right way round?
+                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);
                 auto l_adj = (l.exp()).adj();
                 //std::cout << "l_adj" << std::endl << l_adj << std::endl;
                 //std::cout << std::endl;
-                // problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];  // todo: are those the right
-                // problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];  // TODO: are those the right
-                problems[idx]->Acal_[i + 1] = l_adj;
+                problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
+
+                if (idx == 0)  // make the corrections for the standard riccati solver
+                {
+                    problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
+                    problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
+                    problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
+                }
             }
 
-
             // set the problem pointers
             lqocSolvers[idx]->setProblem(problems[idx]);
 
@@ -210,9 +226,9 @@ int main(int argc, char** argv)
             lqocSolvers[idx]->solve();
 
             // postprocess data
-            lqocSolvers[idx]->computeStatesAndControls();
-            lqocSolvers[idx]->computeFeedbackMatrices();
             lqocSolvers[idx]->compute_lv();
+            lqocSolvers[idx]->computeFeedbackMatrices();
+            lqocSolvers[idx]->computeStatesAndControls();
         }
 
         // retrieve solutions from both solvers
@@ -220,12 +236,48 @@ int main(int argc, char** argv)
         auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
         ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
         ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
-        auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
-        auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = lqocSolvers[1]->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = lqocSolvers[1]->get_lv();
 
-        std::cout << std::endl << std::endl;
+        auto xSol_aug_riccati = lqocSolvers[1]->getSolutionState();
+        auto uSol_aug_riccati = lqocSolvers[1]->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_aug_riccati = lqocSolvers[1]->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_aug_riccati = lqocSolvers[1]->get_lv();
+
+        // compare the quantities
+        for (size_t i = 0; i < lv_sol_riccati.size(); i++)
+        {
+            if (lv_sol_riccati[i].isApprox(lv_sol_aug_riccati[i], 1e-6) == false)
+            {
+                std::cout << lv_sol_riccati[i].transpose() << std::endl;
+                std::cout << lv_sol_aug_riccati[i].transpose() << std::endl;
+                throw std::runtime_error("lv solutions do not match");
+            }
+        }
+        for (size_t i = 0; i < KSol_riccati.size(); i++)
+        {
+            if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
+                throw std::runtime_error("K solutions do not match");
+        }
+
+        for (size_t i = 0; i < uSol_riccati.size(); i++)
+        {
+            if (uSol_riccati[i].isApprox(uSol_aug_riccati[i], 1e-6) == false)
+            {
+                std::cout << "for index " << i << std::endl;
+                std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
+                std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
+                throw std::runtime_error("u solutions do not match");
+            }
+        }
+        for (size_t i = 0; i < xSol_riccati.size(); i++)
+        {
+            if (xSol_riccati[i].isApprox(xSol_aug_riccati[i], 1e-6) == false)
+            {
+                std::cout << xSol_riccati[i].transpose() << std::endl;
+                std::cout << xSol_aug_riccati[i].transpose() << std::endl;
+                throw std::runtime_error("x solutions do not match");
+            }
+        }
+
         //std::cout << std::setprecision(4) << "dx solution from riccati solver and directly added state_traj, iter "
         //          << iter << std::endl;
         //ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
@@ -251,20 +303,28 @@ int main(int argc, char** argv)
         ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
         for (size_t i = 0; i < N; i++)
         {
-            ManifoldState_t::Tangent x_err = x_curr.rminus(x_traj_prev[i]);
+            ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
             // TODO: some term is missing here;
-            const ct::core::ControlVector<control_dim> u =
-                u_traj[i] + lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
-            u_traj[i] = u;
-
-            exampleSystem->computeControlledDynamics(x_curr, i * dt, u, dx);
-            Eigen::Quaterniond old_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
-            x_curr = x_curr + dx;
-            Eigen::Quaterniond new_rot(x_curr.w(), x_curr.x(), x_curr.y(), x_curr.z());
-            std::cout << "m: " << x_curr << "\t tan: " << x_curr.log()
+            if (use_single_shooting)
+                u_traj[i] += lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
+            else
+                u_traj[i] += uSol_riccati[i];
+
+            exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
+            Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
+
+            if (use_single_shooting)
+                x_traj[i + 1] = x_traj[i] + dx;
+            else
+                x_traj[i + 1] = x_traj_prev[i + 1] + xSol_riccati[i + 1];
+
+            Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
+            std::cout << "m: " << x_traj[i + 1] << "\t tan: " << x_traj[i + 1].log()
                       << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
 
-            x_traj[i + 1] = x_curr;
+            b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
+
+            std::cout << "b: " << b[i] << std::endl;
         }
         std::cout << std::endl << std::endl;
     }  // end iter

From 1b1fb281ac71c7d8a368bbd7d28abfa2f48073e0 Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Thu, 30 Apr 2020 12:54:56 +0200
Subject: [PATCH 04/21] backup working example

---
 .../linearizer/DynamicsLinearizerNumDiff.h    |   6 +-
 .../CostFunctionQuadraticSimple-impl.hpp      |  20 +-
 .../CostFunctionQuadraticSimple.hpp           |   2 +
 .../ct/optcon/problem/LQOCProblem-impl.hpp    |   3 +-
 .../solver/lqp/AugGNRiccatiSolver-impl.hpp    |  18 +-
 .../solver/lqp/GNRiccatiSolver-impl.hpp       |  20 +-
 ct_optcon/test/CMakeLists.txt                 |   7 +-
 .../linear/KinematicManifoldLQOCTest.cpp      | 364 +++++++++++
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 616 ++++++++++++++----
 9 files changed, 908 insertions(+), 148 deletions(-)
 create mode 100644 ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp

diff --git a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
index 9662a2bf2..f9e530370 100644
--- a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
+++ b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
@@ -151,7 +151,8 @@ class DynamicsLinearizerNumDiff
             SCALAR dxp = mlog_ph - m_log.coeffs()(i);
 
             t_perturbed = m_log;
-            t_perturbed.coeffs()(i) = mlog_ph;
+            t_perturbed.set_coeff(i, mlog_ph); // TODO
+            //t_perturbed.coeffs()(i) = mlog_ph;
 
             // evaluate dynamics at perturbed state
             dynamics_fct_(retract_fct_(t_perturbed), t, u, res_plus);
@@ -162,7 +163,8 @@ class DynamicsLinearizerNumDiff
                 SCALAR dxm = m_log.coeffs()(i) - mlog_mh;
 
                 t_perturbed = m_log;
-                t_perturbed.coeffs()(i) = mlog_mh;
+                t_perturbed.set_coeff(i, mlog_ph);
+                //t_perturbed.coeffs()(i) = mlog_ph;
 
                 Tangent res_minus;
                 dynamics_fct_(retract_fct_(t_perturbed), t, u, res_minus);
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index 133868cab..2d516c791 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -19,6 +19,8 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
     Q_final_.setZero();
     x_deviation_.setZero();
     u_deviation_.setZero();
+    J_curr_.setIdentity();
+    J_ref_.setIdentity();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -32,6 +34,8 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
 {
     x_deviation_.setZero();
     u_deviation_.setZero();
+    J_curr_.setIdentity();
+    J_ref_.setIdentity();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -42,6 +46,8 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::~CostFunctionQuadraticSimple
 template <typename MANIFOLD, size_t CONTROL_DIM>
 CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(const CostFunctionQuadraticSimple& arg)
     : x_deviation_(arg.x_deviation_),
+      J_curr_(arg.J_curr_),
+      J_ref_(arg.J_ref_),
       x_nominal_(arg.x_nominal_),
       Q_(arg.Q_),
       u_deviation_(arg.u_deviation_),
@@ -67,8 +73,10 @@ void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::setCurrentStateAndContr
     this->u_ = u;
     this->t_ = t;
 
-    this->x_deviation_ = x - x_nominal_;  // TODO: switch order and evluate jacs as well
     this->u_deviation_ = u - u_nominal_;
+
+    //this->x_deviation_ = x - x_nominal_;
+    this->x_deviation_ = x_nominal_.rminus(this->x_, J_ref_, J_curr_);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -82,13 +90,13 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate()
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
 {
-    return Q_ * x_deviation_;
+    return Q_ * J_curr_.transpose() * x_deviation_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeIntermediate() -> state_matrix_t
 {
-    return Q_;
+    return J_curr_ * Q_ * J_curr_.transpose();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -119,18 +127,20 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> S
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
 {
-    Eigen::Matrix<double, STATE_DIM, STATE_DIM> J_curr, J_ref;
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr, J_ref;
     typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
     return Q_final_ * J_curr.transpose() * x_deviation_final;
+    //return - Q_final_  * x_deviation_final;
     //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
-    Eigen::Matrix<double, STATE_DIM, STATE_DIM> J_curr, J_ref;
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr, J_ref;
     typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
     return J_curr * Q_final_ * J_curr.transpose();
+    //return Q_final_ ;
     //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
     //return Q_final_;
 }
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
index d638cafb8..5d1278cd0 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -91,6 +91,8 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<MANIFOLD, CONTR
     typename MANIFOLD::Tangent x_deviation_;
     MANIFOLD x_nominal_;
     state_matrix_t Q_;
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr_; // TODO: rename
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_ref_; // TODO: rename
 
     control_vector_t u_deviation_;
     control_vector_t u_nominal_;
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
index 1178bfde2..c61b8f205 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -326,7 +326,8 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProb
     core::StateMatrix<STATE_DIM, SCALAR> A;
     core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
     linearSystem.getDerivatives(A, B, x_traj.front(), u_traj.front(), 0);
-    A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
+
+    A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A); 
     B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
     b_ = b;
 
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
index f6d3ca33d..a0796490e 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
@@ -63,7 +63,9 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];
+        // Note that we need to transport the state update into the tagent space of k+1
+        this->x_sol_[k + 1] =
+            p.Adj_x_[k + 1].transpose() * (p.A_[k] * this->x_sol_[k] + p.B_[k] * this->u_sol_[k] + p.b_[k]);
     }
 }
 
@@ -157,17 +159,17 @@ void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
     LQOCProblem_t& p = *this->lqocProblem_;
 
     S_[k] = p.Q_[k];
-    S_[k].noalias() += p.A_[k].transpose() * S_t_[k + 1] * p.A_[k];
-    S_[k].noalias() -= this->L_[k].transpose() * Hi_[k] * this->L_[k];
+    S_[k]/*.noalias()*/ += p.A_[k].transpose() * S_t_[k + 1] * p.A_[k]; // TODO: bring back all the noalias()
+    S_[k]/*.noalias()*/ -= this->L_[k].transpose() * Hi_[k] * this->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_t_[k + 1];
-    sv_[k].noalias() += p.A_[k].transpose() * S_t_[k + 1] * p.b_[k];
-    sv_[k].noalias() += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
-    sv_[k].noalias() += this->L_[k].transpose() * gv_[k];
-    sv_[k].noalias() += G_[k].transpose() * this->lv_[k];
+    sv_[k]/*.noalias()*/ += p.A_[k].transpose() * sv_t_[k + 1];
+    sv_[k]/*.noalias()*/ += p.A_[k].transpose() * S_t_[k + 1] * p.b_[k];
+    sv_[k]/*.noalias()*/ += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
+    sv_[k]/*.noalias()*/ += this->L_[k].transpose() * gv_[k];
+    sv_[k]/*.noalias()*/ += G_[k].transpose() * this->lv_[k];
 
     if (ct::core::is_euclidean<MANIFOLD>::value)  // euclidean case
     {
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 9a94d6a4b..ea9748b63 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -63,10 +63,12 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backwards pass"
-        StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backwards pass"
-        typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backwards pass"
-        this->x_sol_[k + 1] = A_orig * this->x_sol_[k] + B_orig * (this->u_sol_[k]) + b_orig;
+        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backward pass"
+        StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backward pass"
+        typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backward pass"
+        // Note that we need to transport the state update into the tagent space of k+1
+        this->x_sol_[k + 1] =
+            p.Adj_x_[k + 1].transpose() * (A_orig * this->x_sol_[k] + B_orig * (this->u_sol_[k]) + b_orig);
     }
 }
 
@@ -153,11 +155,11 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t 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() += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
-    sv_[k].noalias() += this->L_[k].transpose() * gv_[k];
-    sv_[k].noalias() += G_[k].transpose() * this->lv_[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()*/ += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
+    sv_[k]/*.noalias()*/ += this->L_[k].transpose() * gv_[k];
+    sv_[k]/*.noalias()*/ += G_[k].transpose() * this->lv_[k]; // TODO: bring back all the noalias()
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/test/CMakeLists.txt b/ct_optcon/test/CMakeLists.txt
index 49c1c189a..1e54468b1 100644
--- a/ct_optcon/test/CMakeLists.txt
+++ b/ct_optcon/test/CMakeLists.txt
@@ -53,8 +53,13 @@ if(BUILD_TESTS)
     #package_add_test(dms_test_all_var dms/oscillator/oscDMSTestAllVariants.cpp)
     #package_add_test(system_interface_test system_interface/SystemInterfaceTest.cpp)
     
+    #add_executable(KinematicManifoldLQOCTest solver/linear/KinematicManifoldLQOCTest.cpp)
+    #target_link_libraries(KinematicManifoldLQOCTest ct_optcon)
+    #set_target_properties(KinematicManifoldLQOCTest PROPERTIES FOLDER test)
+    #list(APPEND UNIT_TEST_TARGETS KinematicManifoldLQOCTest)    
+    
     add_executable(ManifoldLQOCTest solver/linear/ManifoldLQOCTest.cpp)
-    target_link_libraries(ManifoldLQOCTest ct_optcon)
+    target_link_libraries(ManifoldLQOCTest ct_optcon ct_core)
     set_target_properties(ManifoldLQOCTest PROPERTIES FOLDER test)
     list(APPEND UNIT_TEST_TARGETS ManifoldLQOCTest)
 
diff --git a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
new file mode 100644
index 000000000..227bfaaef
--- /dev/null
+++ b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
@@ -0,0 +1,364 @@
+
+#include <ct/optcon/optcon.h>
+#include <ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp>
+#include <ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp>
+
+//#include "matplotlibcpp.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+const bool verbose = true;
+
+using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
+const size_t state_dim = ManifoldState_t::TangentDim;
+const size_t control_dim = 3;
+
+class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    DiscrSO3LTITestSystem() {}
+    virtual void computeControlledDynamics(const ManifoldState_t& m,
+        const Time_t& n,
+        const ct::core::ControlVector<control_dim>& u,
+        ManifoldState_t::Tangent& dx) override
+    {
+        dx = u;
+    }
+
+    virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(); }
+    /**
+     * @brief the log operator is defined as expressing the tangent vector w.r.t. m_ref_
+     */
+    virtual ManifoldState_t::Tangent lift(const ManifoldState_t& m) override
+    {
+        //throw std::runtime_error("not impl");
+        return ManifoldState_t::Tangent::Zero();  // the system stays where it is (combination with A = identity())
+        /*return m.rminus(m);*/
+    }
+    // virtual ManifoldState_t retract(const ManifoldState_t::Tangent& t) override {
+    //     throw std::runtime_error("not implemetned.");
+    //     /*return m_ref_.rplus(t);*/ }
+protected:
+    //ManifoldState_t m_ref_;
+};
+
+
+// TODO: make this a unit test
+void testParallelTransport()
+{
+    auto origin = manif::SE2<double>(0, 0, 0);
+    Eigen::Matrix3d b;
+    b.setIdentity();  // some basis, assumed to be expressed in the manifolds over which we iterate
+
+    for (double theta = M_PI / 2; theta <= 2 * M_PI; theta += M_PI / 2)
+    {
+        std::cout << "case theta = " << theta << std::endl;
+        auto m = manif::SE2<double>(0, 0, theta);
+        Eigen::Matrix3d Jl, Jr;  // jacobians
+        auto d_rminus = m.rminus(origin, Jl, Jr);
+        auto adj_rminus = d_rminus.exp().adj();  // the adjoint for transportation to the origin
+
+        auto d_between = m.between(origin);
+        auto adj_between = d_rminus.exp().adj();
+
+        // ASSERT_TRUE(adj_rminus.isApprox(adj_between));
+
+        std::cout << adj_rminus * b << std::endl << std::endl;   // express basis b in origin-manifold
+        std::cout << adj_between * b << std::endl << std::endl;  // express basis b in origin-manifold
+        // std::cout << Jr * Jl * b << std::endl << std::endl;  // TODO: how does the Jac relate to the adjoint here?
+        std::cout << std::endl;
+    }
+
+    // auto m_pi_2 = manif::SE2<double>(0, 0, M_PI / 2);
+    // auto m_pi = manif::SE2<double>(0, 0, M_PI);
+    // auto m_mpi_2 = manif::SE2<double>(0, 0, -M_PI / 2);
+    //
+    //
+    // std::cout << m_0.adj() * b << std::endl << std::endl;
+    // std::cout << m_pi_2.adj() * b << std::endl << std::endl;
+    // std::cout << m_pi.adj() * b << std::endl << std::endl;
+    // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
+}
+
+int main(int argc, char** argv)
+{
+    std::cout << std::fixed;
+
+    const bool use_single_shooting = true;  // toggle between single and multiple shooting
+
+    const size_t N = 250;
+    const double dt = 0.5;
+
+    const ManifoldState_t x0 = manif::SO3<double>(3.1, 0, 0);
+    ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
+    ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
+        N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
+    ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
+    for (size_t i = 0; i < N; i++)
+        u_traj[i] = ct::core::ControlVector<control_dim>::Random()* 0.01;
+
+    // choose a random initial state
+    // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
+    for (size_t i = 1; i < N + 1; i++)
+    {
+        x_traj[i] = ManifoldState_t::Random();
+    }
+
+    // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
+        new GNRiccatiSolver<ManifoldState_t, control_dim>);
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> augGnRiccatiSolver(
+        new AugGNRiccatiSolver<ManifoldState_t, control_dim>);
+
+    // store them, and identifying names, in a vectors
+    std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
+    lqocSolvers.push_back(gnRiccatiSolver);
+    lqocSolvers.push_back(augGnRiccatiSolver);
+    std::vector<std::string> solverNames = {"Riccati", "AugRiccati"};
+
+    // create linear-quadratic optimal control problem containers
+    std::vector<std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>>> problems;
+    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem1(
+        new LQOCProblem<ManifoldState_t, control_dim>(N));
+    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem2(
+        new LQOCProblem<ManifoldState_t, control_dim>(N));
+
+    problems.push_back(lqocProblem1);
+    problems.push_back(lqocProblem2);
+
+    // create a discrete-time manifold system
+    std::shared_ptr<ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
+        new DiscrSO3LTITestSystem());
+    std::shared_ptr<ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>> linearizer(
+        new ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>(exampleSystem));
+
+
+    // create a cost function
+    Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
+    Eigen::Matrix<double, control_dim, control_dim> R;
+    Q_final << 10000, 0, 0, 0, 10000, 0, 0, 0, 10000;
+    Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    //Q.setZero();
+    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    ManifoldState_t x_final = manif::SO3<double>(0, 0, 0);
+    std::cout << "desired final state: " << x_final << std::endl;
+    ManifoldState_t x_nominal = x0;
+    ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
+    std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
+        new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
+
+
+    // integrate an initial state with the open-loop system to get initial trajectories
+    ManifoldState_t x_curr;
+    ManifoldState_t::Tangent dx;
+    x_curr = x0;
+    x_traj.front() = x0;
+    std::cout << "integrate an random initial state with the unstable system" << std::endl;
+    std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    for (size_t i = 0; i < N; i++)
+    {
+        exampleSystem->computeControlledDynamics(x_traj[i], 0, u_traj[i], dx);
+        x_curr = x_traj[i] + dx;
+        if (use_single_shooting)
+            x_traj[i + 1] = x_curr;
+        b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
+        std::cout << "b: " << b[i] << std::endl;
+        std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    }
+
+    size_t nIter = 35;
+    for (size_t iter = 0; iter < nIter; iter++)
+    {
+        // initialize the optimal control problems for both solvers
+        problems[0]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
+        problems[1]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
+
+
+        // HACKY corrections // TODO: move somewhere meaningful
+        for (size_t idx : {0, 1})
+        {
+            // intermediate stages cost transportation
+            for (size_t i = 0; i < N; i++)
+            {
+                auto e = x_nominal.rminus(x_traj[i]);
+                // compute PT matrix w.r.t. current ref traj // TODO: clarify formulation of error in cost function
+                auto e_adj = (e.exp()).adj();
+                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] *
+                                       e_adj.transpose();  // TODO: sort out that thing with the cost function
+                problems[idx]->qv_[i] = e_adj * problems[idx]->qv_[i];
+            }
+            // terminal stage transportation
+            // TODO: clarifiy formulation of error in cost function
+            auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.t. current ref traj
+            auto e_adj = (e.exp()).adj();
+            //std::cout << "cost adj" << std::endl << e_adj << std::endl;
+            problems[idx]->Q_.back() = e_adj * problems[idx]->Q_.back() * e_adj.transpose();
+            problems[idx]->qv_.back() = e_adj * problems[idx]->qv_.back();
+
+            // dynamics transportation
+            for (size_t i = 0; i < N; i++)
+            {
+                // std::cout << "dyn transport matrices" << std::endl;
+                Eigen::Matrix3d Jl, Jr;
+                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);
+                auto l_adj = (l.exp()).adj();
+                //std::cout << "l_adj" << std::endl << l_adj << std::endl;
+                //std::cout << std::endl;
+                problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
+
+                if (idx == 0)  // make the corrections for the standard riccati solver
+                {
+                    problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
+                    problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
+                    problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
+                }
+
+                // now just a test // todo: remove later
+                //auto temp_l = x_traj[i + 1].between(x_traj[i]);
+                //auto temp_l_adj = temp_l.adj();
+                //if (!(l_adj.isApprox(temp_l_adj.transpose(), 1e-5)))
+                //{
+                //    std::cout << std::setprecision(10) << "l_adj: " << std::endl << l_adj << std::endl;
+                //    std::cout << std::setprecision(10) << "temp_l_adj: " << std::endl << temp_l_adj << std::endl;
+                //    throw std::runtime_error("No- that does not work.");
+                //}
+            }
+
+            // set the problem pointers
+            lqocSolvers[idx]->setProblem(problems[idx]);
+
+            // allocate memory (if required)
+            lqocSolvers[idx]->initializeAndAllocate();
+
+            // solve the problems...
+            lqocSolvers[idx]->solve();
+
+            // postprocess data
+            lqocSolvers[idx]->compute_lv();
+            lqocSolvers[idx]->computeFeedbackMatrices();
+            lqocSolvers[idx]->computeStatesAndControls();
+        }
+
+        // retrieve solutions from both solvers
+        auto xSol_riccati = lqocSolvers[0]->getSolutionState();
+        auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
+
+        auto xSol_aug_riccati = lqocSolvers[1]->getSolutionState();
+        auto uSol_aug_riccati = lqocSolvers[1]->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_aug_riccati = lqocSolvers[1]->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_aug_riccati = lqocSolvers[1]->get_lv();
+
+        // compare the quantities
+        //for (size_t i = 0; i < lv_sol_riccati.size(); i++)
+        //{
+        //    if ((lv_sol_riccati[i] - lv_sol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << std::setprecision(10) << lv_sol_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << lv_sol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("lv solutions do not match");
+        //    }
+        //}
+        //for (size_t i = 0; i < KSol_riccati.size(); i++)
+        //{
+        //    if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
+        //        throw std::runtime_error("K solutions do not match");
+        //}
+//
+        //for (size_t i = 0; i < uSol_riccati.size(); i++)
+        //{
+        //    if ((uSol_riccati[i] - uSol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << "for index " << i << std::endl;
+        //        std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("u solutions do not match");
+        //    }
+        //}
+        //for (size_t i = 0; i < xSol_riccati.size(); i++)
+        //{
+        //    if ((xSol_riccati[i] - xSol_aug_riccati[i]).coeffs().array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << xSol_riccati[i].transpose() << std::endl;
+        //        std::cout << xSol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("x solutions do not match");
+        //    }
+        //}
+
+        //std::cout << std::setprecision(4) << "dx solution from riccati solver and directly added state_traj, iter "
+        //          << iter << std::endl;
+        //ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
+        //for (size_t j = 0; j < xSol_riccati.size(); j++)
+        //{
+        //    x_solver_direct[j] = x_traj[j] + xSol_riccati[j];
+        //    double angularDiff = 0;
+        //    if (j > 0)
+        //    {
+        //        Eigen::Quaterniond old_rot(x_solver_direct[j - 1].w(), x_solver_direct[j - 1].x(),
+        //            x_solver_direct[j - 1].y(), x_solver_direct[j - 1].z());
+        //        Eigen::Quaterniond new_rot(
+        //            x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
+        //        angularDiff = old_rot.angularDistance(new_rot);
+        //    }
+        //    std::cout << "m:" << x_solver_direct[j] << "\t dx:" << xSol_riccati[j].transpose()
+        //              << "\t -- rot diff norm(): " << angularDiff << std::endl;
+        //}
+
+        std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
+        x_curr = x0;
+        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
+        double d_cum_sum = 0;
+        double dx_cum_sum = 0;
+        double cost_sum = 0;
+        for (size_t i = 0; i < N; i++)
+        {
+            dx.setZero();
+            Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
+
+            if (use_single_shooting)
+            {
+                // TODO: some term is missing here;
+                ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
+                u_traj[i] += lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
+                exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
+                x_traj[i + 1] = x_traj[i] + dx;
+            }
+            else  // multiple shooting
+            {
+                u_traj[i] += uSol_riccati[i];
+                x_traj[i + 1] += xSol_riccati[i + 1];
+                exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
+            }
+
+            Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
+            //std::cout << "m: " << x_traj[i + 1] << "\t dx: " << xSol_riccati[i + 1]
+            //          << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
+
+            // compute defect
+            b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
+
+            // compute update norms
+            d_cum_sum += b[i].coeffs().norm();
+            dx_cum_sum += xSol_riccati[i + 1].coeffs().norm();
+            //std::cout << "b: " << b[i] << std::endl;
+            // compute running cost
+            costFunction->setCurrentStateAndControl(x_traj[i], u_traj[i], i * dt);
+            cost_sum += costFunction->evaluateIntermediate();
+        }
+
+        // compute terminal cost
+        costFunction->setCurrentStateAndControl(x_traj.back(), u_traj.back(), N * dt);
+        cost_sum += costFunction->evaluateTerminal();
+
+        std::cout << "d_norm: \t " << d_cum_sum << "\t dx_norm: \t" << dx_cum_sum << "\t cost: " << cost_sum
+                  << std::endl;
+        std::cout << std::endl << std::endl;
+    }  // end iter
+    return 1;
+}
\ No newline at end of file
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 83fdeb736..e7dd2db10 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -3,107 +3,454 @@
 #include <ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp>
 #include <ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp>
 
+
 using namespace ct::core;
 using namespace ct::optcon;
+const bool verbose = true;
 
 
-const bool verbose = true;
+template <typename POS_MAN, typename VEL_MAN, typename POS_TANGENT, typename VEL_TANGENT>
+class CompositeManifold;  // forward declaratoin
+
+template <typename POS_MAN, typename VEL_MAN, typename POS_TANGENT, typename VEL_TANGENT>
+class CompositeManifoldTangent
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    static constexpr size_t DoF = POS_TANGENT::DoF + VEL_TANGENT::DoF;
+    using Scalar = double;
+    using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
+    using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
+
+    CompositeManifoldTangent() : t_pos_(POS_TANGENT::Zero()), t_vel_(VEL_TANGENT::Zero()) { coeffs_.setZero(); }
+    CompositeManifoldTangent(const POS_TANGENT& p, const VEL_TANGENT& v) : t_pos_(p), t_vel_(v)
+    {
+        coeffs_ << t_pos_.coeffs(), t_vel_.coeffs();
+    }
+
+    template <typename _EigenDerived>
+    CompositeManifoldTangent(const Eigen::MatrixBase<_EigenDerived>& v)
+        : t_pos_(v.template head<POS_TANGENT::DoF>()), t_vel_(v.template tail<VEL_TANGENT::DoF>()), coeffs_(v)
+    {
+        // TODO a dimension assert would not hurt
+    }
+
+    CompositeManifoldTangent(const CompositeManifoldTangent& resulting_t)
+        : t_pos_(resulting_t.t_pos_), t_vel_(resulting_t.t_vel_), coeffs_(resulting_t.coeffs_)
+    {
+    }
+
+    static CompositeManifoldTangent Zero()
+    {
+        return CompositeManifoldTangent(POS_TANGENT::Zero(), VEL_TANGENT::Zero());
+    }
+    static CompositeManifoldTangent Random()
+    {
+        return CompositeManifoldTangent(POS_TANGENT::Random(), VEL_TANGENT::Random());
+    }
+
+    void setRandom()
+    {
+        t_pos_.setRandom();
+        t_vel_.setRandom();
+        coeffs_ << t_pos_.coeffs(), t_vel_.coeffs();
+    }
+    void setZero()
+    {
+        t_pos_.setZero();
+        t_vel_.setZero();
+        coeffs_.setZero();
+    }
+    const Eigen::Matrix<Scalar, DoF, 1>& coeffs() const { return coeffs_; }
+    void set_coeffs(const Eigen::Matrix<Scalar, DoF, 1>& c)
+    {
+        coeffs_ = c;
+        t_pos_.coeffs() = c.template head<POS_TANGENT::DoF>();
+        t_vel_.coeffs() = c.template tail<VEL_TANGENT::DoF>();
+    }
+
+    void set_coeff(const int i, const Scalar val)
+    {
+        coeffs_(i) = val;
+        t_pos_.coeffs() = coeffs_.template head<POS_TANGENT::DoF>();
+        t_vel_.coeffs() = coeffs_.template tail<VEL_TANGENT::DoF>();
+    }
+
+    CompositeManifoldTangent operator-(const CompositeManifoldTangent& tb) const
+    {
+        CompositeManifoldTangent res;
+        res.set_pos(pos() - tb.pos());
+        res.set_vel(vel() - tb.vel());
+        return res;
+    }
+
+    CompositeManifoldTangent operator+(const CompositeManifoldTangent& tb) const
+    {
+        CompositeManifoldTangent res;
+        res.set_pos(pos() + tb.pos());
+        res.set_vel(vel() + tb.vel());
+        return res;
+    }
+    CompositeManifoldTangent operator+=(const CompositeManifoldTangent& tb)
+    {
+        set_pos(pos() + tb.pos());
+        set_vel(vel() + tb.vel());
+        return *this;
+    }
+
+    CompositeManifold<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT> exp(OptJacobianRef J_m_t = OptJacobianRef{}) const
+    {
+        //std::cout << "now calling exp..." << std::endl;
+        if (J_m_t)
+        {
+            throw std::runtime_error("J-m-resulting_t in tangent exp not impl yet.");
+        }
+
+        CompositeManifold<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT> c(
+            this->t_pos_.exp(), this->t_pos_.exp().adj().transpose() * this->t_vel_.coeffs());
+        return c;
+    }
+
+    auto transpose() const { return coeffs().transpose(); }
+    const POS_TANGENT& pos() const { return t_pos_; }
+    void set_pos(const POS_TANGENT& p)
+    {
+        t_pos_ = p;
+        coeffs_.template head<POS_TANGENT::DoF>() = p.coeffs();
+    }
+    void set_vel(const VEL_TANGENT& v)
+    {
+        t_vel_ = v;
+        coeffs_.template tail<VEL_TANGENT::DoF>() = v.coeffs();
+    }
+    const VEL_TANGENT& vel() const { return t_vel_; }
+    //Scalar& operator()(int idx) { return coeffs()(idx); }
+    const Scalar& operator()(int idx) const { return coeffs()(idx); }
+    //auto noalias() { return CompositeManifoldTangent<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT>(*this); /* coeffs().noalias();*/ }  // TODO: sort this out
+protected:
+    POS_TANGENT t_pos_;
+    VEL_TANGENT t_vel_;
+    Eigen::Matrix<Scalar, DoF, 1> coeffs_;
+};
+
+template <typename _Stream, typename t1, typename t2, typename t3, typename t4>
+_Stream& operator<<(_Stream& s, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+{
+    s << resulting_t.coeffs().transpose();
+    return s;
+}
+
+template <class _EigenDerived, typename t1, typename t2, typename t3, typename t4>
+auto operator*(const Eigen::MatrixBase<_EigenDerived>& J, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+{
+    return J * resulting_t.coeffs();
+}
+
+template <typename t1, typename t2, typename t3, typename t4>
+auto operator*(const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t, const double& s)
+{
+    return resulting_t.coeffs() * s;
+}
+
+template <class _EigenDerived, typename t1, typename t2, typename t3, typename t4>
+auto operator+(const Eigen::MatrixBase<_EigenDerived>& tb, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+{
+    CompositeManifoldTangent<t1, t2, t3, t4> res;
+    CompositeManifoldTangent<t1, t2, t3, t4> rhs(tb);
+    res.set_pos(resulting_t.pos() + rhs.pos());
+    res.set_vel(resulting_t.vel() + rhs.vel());
+    return res;
+}
+
+
+template <typename POS_MAN, typename VEL_MAN, typename POS_TANGENT, typename VEL_TANGENT>
+class CompositeManifold
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    using Tangent = CompositeManifoldTangent<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT>;
+    static constexpr size_t TangentDim = Tangent::DoF;
+    static constexpr size_t Dim = POS_MAN::Dim + VEL_MAN::Dim + 1;  // todo: this is hacky
+
+    using Scalar = double;
+    using Jacobian = Eigen::Matrix<Scalar, TangentDim, TangentDim>;
+    using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
+
+    // get ManifoldState templated on a different scalar from this expression
+    template <typename OtherScalar>
+    using RedefineScalar = CompositeManifold<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT>;
+
+    CompositeManifold() : m_pos_(POS_MAN::Identity()), m_vel_(VEL_MAN::Identity()) {}
+    CompositeManifold(const POS_MAN& p, const VEL_MAN& v) : m_pos_(p), m_vel_(v) {}
+    CompositeManifold(const CompositeManifold& arg) : m_pos_(arg.m_pos_), m_vel_(arg.m_vel_) {}
+    static CompositeManifold NeutralElement() { return CompositeManifold::Identity(); }
+    static CompositeManifold Identity() { return CompositeManifold(POS_MAN::Identity(), VEL_MAN::Identity()); }
+    static CompositeManifold Random() { return CompositeManifold(POS_MAN::Random(), VEL_MAN::Random()); }
+    void setIdentity()
+    {
+        m_pos_.setIdentity();
+        m_vel_.setIdentity();
+    }
+
+    const Eigen::Matrix<Scalar, Dim, 1> coeffs() const  // todo: return by value is bad
+    {
+        Eigen::Matrix<Scalar, Dim, 1> c;
+        c << m_pos_.coeffs(), m_vel_.coeffs();
+        return c;
+    }
+
+    CompositeManifold operator+(const Tangent& resulting_t) const { return rplus(resulting_t); }
+    //CompositeManifold operator+=(const Tangent& resulting_t)
+    //{
+    //    *this = this->rplus(resulting_t);
+    //    return *this;  // todo test
+    //}
+
+    /**
+     * @brief right plus for composite manifold
+     * NOTE: the operator also has to parallel transport velocities to the new tangent plane.
+     * Since for rplus, the rhs is expressed in the tangent space of the lhs, we choose: "first add, then transport"
+     */
+    CompositeManifold rplus(const Tangent& resulting_t, OptJacobianRef Jl = {}, OptJacobianRef Jr = {}) const
+    {
+        typename POS_MAN::Jacobian pJl, pJr;
+        typename VEL_MAN::Jacobian vJl, vJr;
+
+        VEL_MAN v_new_local = vel().rplus(resulting_t.vel(), vJl, vJr);
+
+        VEL_MAN v_new_transported(resulting_t.pos().exp().adj().transpose() * v_new_local.coeffs());
+
+        CompositeManifold c;
+        c.vel() = v_new_transported;
+        //c.vel() = c.vel().rplus(resulting_t.vel(), vJl, vJr);
+        c.pos() = pos().rplus(resulting_t.pos(), pJl, pJr);  // todo make jacobians optional
+
+        if (Jl)
+        {
+            Jl->setZero();
+            (*Jl).template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = pJl;
+            (*Jl).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = vJl;
+        }
+
+        if (Jr)
+        {
+            Jr->setZero();
+            (*Jr).template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = pJr;
+            (*Jr).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() =
+                vJr;  // TODO: what will the velocity jacobians become?
+        }
 
-using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
+        return c;
+    }
+
+
+    /**
+     * @brief right minus for composite manifold
+     * NOTE: the operator also has to express velocities in the rhs tangent plane.
+     * Since for rminus, the resulting difference is expressed in the tangent space of the rhs, we choose: "first transport, then substract"
+     */
+    Tangent rminus(const CompositeManifold& rhs, OptJacobianRef Jl = {}, OptJacobianRef Jr = {}) const
+    {
+        typename POS_MAN::Jacobian pJl, pJr;
+        typename VEL_MAN::Jacobian vJl, vJr;
+
+        Tangent resulting_t;
+        resulting_t.set_pos(pos().rminus(rhs.pos(), pJl, pJr));  // todo make jacobians optional
+        VEL_MAN v_lhs_transported(resulting_t.pos().exp().adj() * vel().coeffs());
+
+        VEL_TANGENT v_new_local = v_lhs_transported.rminus(rhs.vel(), vJl, vJr);
+        resulting_t.set_vel(v_new_local);
+
+        if (Jl)
+        {
+            Jl->setIdentity();
+            (*Jl).template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = pJl;
+            (*Jl).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = vJl;
+        }
+
+        if (Jr)
+        {
+            Jr->setIdentity();
+            (*Jr).template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = pJr;
+            (*Jr).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = vJr;
+        }
+
+        return resulting_t;
+    }
+
+
+    Tangent log(OptJacobianRef J_t_m = {}) const
+    {
+        if (J_t_m)
+            throw std::runtime_error("J_t_m compuation not defined.");
+
+        return rminus(CompositeManifold::Identity(), J_t_m);
+        //resulting_t.set_pos(m_pos_.log());
+        //resulting_t.set_vel(m_vel_.log());
+        //return resulting_t;
+    }
+
+    /**
+     * @brief The question
+     * The fundamental question remains: what is the velocity adjoint?
+     *  - multiple shooting: in the forward update rule, the computed velocities want to be transported to the next states manifold coordinate system
+     *  - single shooting: seems to be much more stable when velocity part adjoint is identity -- is the value function more stable???
+     */
+    Jacobian adj() const
+    {
+        Jacobian J = Jacobian::Zero();
+        J.template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = m_pos_.adj();
+        J.template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() =
+            m_pos_.adj();  //.setIdentity();  // = m_pos_.adj(); // TODO: what exactly is it?
+        return J;
+    }
+
+    POS_MAN& pos() { return m_pos_; }
+    const POS_MAN& pos() const { return m_pos_; }
+    VEL_MAN& vel() { return m_vel_; }
+    const VEL_MAN& vel() const { return m_vel_; }
+protected:
+    POS_MAN m_pos_;
+    VEL_MAN m_vel_;
+};
+template <typename _Stream, typename t1, typename t2, typename t3, typename t4>
+_Stream& operator<<(_Stream& s, const CompositeManifold<t1, t2, t3, t4>& m)
+{
+    s << m.coeffs().transpose();
+    return s;
+}
+
+
+using ManifoldState_t = CompositeManifold<manif::SO3d, manif::R3d, manif::SO3Tangentd, manif::R3Tangentd>;
 const size_t state_dim = ManifoldState_t::TangentDim;
 const size_t control_dim = 3;
 
+
 class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    DiscrSO3LTITestSystem() {}
+    DiscrSO3LTITestSystem(double dt) : dt_(dt) {}
     virtual void computeControlledDynamics(const ManifoldState_t& m,
         const Time_t& n,
         const ct::core::ControlVector<control_dim>& u,
         ManifoldState_t::Tangent& dx) override
     {
-        dx = u;
+        dx.setZero();
+        dx.set_vel(dt_ * u);                 // velocity increment
+        dx.set_pos(dt_ * m.vel().coeffs());  // position increment
     }
 
-    virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(); }
+    virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(*this); }
     /**
-     * @brief the log operator is defined as expressing the tangent vector w.r.t. m_ref_
+     * @brief the log operator is defined as expressing the tangent vector w.r.resulting_t. m_ref_
      */
     virtual ManifoldState_t::Tangent lift(const ManifoldState_t& m) override
     {
-        //throw std::runtime_error("not impl");
-        return ManifoldState_t::Tangent::Zero();  // the system stays where it is (combination with A = identity())
-        /*return m.rminus(m);*/
+        ManifoldState_t::Tangent resulting_t = ManifoldState_t::Tangent::Zero();
+        //resulting_t.set_pos( "zero...");
+        resulting_t.set_vel(m.vel().log());
+        return resulting_t;
     }
-    // virtual ManifoldState_t retract(const ManifoldState_t::Tangent& t) override {
-    //     throw std::runtime_error("not implemetned.");
-    //     /*return m_ref_.rplus(t);*/ }
+
+    //virtual ManifoldState_t retract(const ManifoldState_t::Tangent& resulting_t) override {
+    //     throw std::runtime_error(" retract not implemetned.");
+    //     /*return m_ref_.rplus(resulting_t);*/ }
 protected:
     //ManifoldState_t m_ref_;
+    double dt_;
 };
 
 
 // TODO: make this a unit test
-void testParallelTransport()
-{
-    auto origin = manif::SE2<double>(0, 0, 0);
-    Eigen::Matrix3d b;
-    b.setIdentity();  // some basis, assumed to be expressed in the manifolds over which we iterate
-
-    for (double theta = M_PI / 2; theta <= 2 * M_PI; theta += M_PI / 2)
-    {
-        std::cout << "case theta = " << theta << std::endl;
-        auto m = manif::SE2<double>(0, 0, theta);
-        Eigen::Matrix3d Jl, Jr;  // jacobians
-        auto d_rminus = m.rminus(origin, Jl, Jr);
-        auto adj_rminus = d_rminus.exp().adj();  // the adjoint for transportation to the origin
-
-        auto d_between = m.between(origin);
-        auto adj_between = d_rminus.exp().adj();
-
-        // ASSERT_TRUE(adj_rminus.isApprox(adj_between));
+//void testParallelTransport()
+//{
+//    auto origin = manif::SE2<double>(0, 0, 0);
+//    Eigen::Matrix3d b;
+//    b.setIdentity();  // some basis, assumed to be expressed in the manifolds over which we iterate
+//
+//    for (double theta = M_PI / 2; theta <= 2 * M_PI; theta += M_PI / 2)
+//    {
+//        std::cout << "case theta = " << theta << std::endl;
+//        auto m = manif::SE2<double>(0, 0, theta);
+//        Eigen::Matrix3d Jl, Jr;  // jacobians
+//        auto d_rminus = m.rminus(origin, Jl, Jr);
+//        auto adj_rminus = d_rminus.exp().adj();  // the adjoint for transportation to the origin
+//
+//        auto d_between = m.between(origin);
+//        auto adj_between = d_rminus.exp().adj();
+//
+//        // ASSERT_TRUE(adj_rminus.isApprox(adj_between));
+//
+//        std::cout << adj_rminus * b << std::endl << std::endl;   // express basis b in origin-manifold
+//        std::cout << adj_between * b << std::endl << std::endl;  // express basis b in origin-manifold
+//        // std::cout << Jr * Jl * b << std::endl << std::endl;  // TODO: how does the Jac relate to the adjoint here?
+//        std::cout << std::endl;
+//    }
+//
+//    // auto m_pi_2 = manif::SE2<double>(0, 0, M_PI / 2);
+//    // auto m_pi = manif::SE2<double>(0, 0, M_PI);
+//    // auto m_mpi_2 = manif::SE2<double>(0, 0, -M_PI / 2);
+//    //
+//    //
+//    // std::cout << m_0.adj() * b << std::endl << std::endl;
+//    // std::cout << m_pi_2.adj() * b << std::endl << std::endl;
+//    // std::cout << m_pi.adj() * b << std::endl << std::endl;
+//    // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
+//}
 
-        std::cout << adj_rminus * b << std::endl << std::endl;   // express basis b in origin-manifold
-        std::cout << adj_between * b << std::endl << std::endl;  // express basis b in origin-manifold
-        // std::cout << Jr * Jl * b << std::endl << std::endl;  // TODO: how does the Jac relate to the adjoint here?
-        std::cout << std::endl;
-    }
-
-    // auto m_pi_2 = manif::SE2<double>(0, 0, M_PI / 2);
-    // auto m_pi = manif::SE2<double>(0, 0, M_PI);
-    // auto m_mpi_2 = manif::SE2<double>(0, 0, -M_PI / 2);
+int main(int argc, char** argv)
+{
+    //ManifoldState_t x_test;
+    //x_test.setIdentity();
+    //Eigen::Vector3d vel_init(1.0, 0.0, 0.0);
+    //x_test.vel() = manif::R3d(vel_init);
+    ////x0.pos() = manif::SO3<double>(0, 0, M_PI);
+    //std::cout << "m: " << x_test << std::endl;
+    //ManifoldState_t::Tangent t_test;
+    //t_test.set_coeff(2, M_PI / 2);
+    //t_test.set_coeff(3, 1);
     //
+    //std::cout << "resulting_t: " << t_test << std::endl;
+    //ManifoldState_t newX = x_test.rplus(t_test);
+    //std::cout << "newX: " << newX << std::endl;
+    //auto t_reverse = newX.rminus(x_test);
+    //std::cout << "t_reverse:" << t_reverse << std::endl;
+    //std::cout << "t_reverse.exp() " << t_reverse.exp() << std::endl;
+    //std::cout << "t_reverse.exp().log() " << t_reverse.exp().log() << std::endl;
     //
-    // std::cout << m_0.adj() * b << std::endl << std::endl;
-    // std::cout << m_pi_2.adj() * b << std::endl << std::endl;
-    // std::cout << m_pi.adj() * b << std::endl << std::endl;
-    // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
-}
+    //std::cout << "parallel transport study: " << std::endl;
+    //auto ADJ = t_reverse.exp().adj();
+    //std::cout << "adj: " << ADJ << std::endl;
+    //std::cout << "adj*t_test: " << ADJ * t_test << std::endl;
+    //exit(0);
 
-int main(int argc, char** argv)
-{
     std::cout << std::fixed;
 
-    const bool use_single_shooting = false;  // toggle between single and multiple shooting
+    const bool use_single_shooting = true;  // toggle between single and multiple shooting
 
-    const size_t N = 25;
-    const double dt = 0.05;
+    const size_t N = 17;
+    const double dt = 0.1;
 
-    const ManifoldState_t x0 = manif::SO3<double>(M_PI, 0, M_PI);
+    ManifoldState_t x0;
+    x0.setIdentity();
+    x0.pos() = manif::SO3<double>(M_PI/2, M_PI/2, 0.0);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
     ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
         N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
     ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
     for (size_t i = 0; i < N; i++)
-        u_traj[i] = ct::core::ControlVector<control_dim>::Random() * dt;
+    {
+        u_traj[i] = ct::core::ControlVector<control_dim>::Random();
+    }
 
     // choose a random initial state
     // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
     for (size_t i = 1; i < N + 1; i++)
     {
-        x_traj[i] = (ManifoldState_t::Random().log() * 0.2).exp();
+        x_traj[i] = ManifoldState_t::Random();
     }
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
@@ -130,7 +477,7 @@ int main(int argc, char** argv)
 
     // create a discrete-time manifold system
     std::shared_ptr<ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
-        new DiscrSO3LTITestSystem());
+        new DiscrSO3LTITestSystem(dt));
     std::shared_ptr<ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>> linearizer(
         new ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>(exampleSystem));
 
@@ -138,13 +485,16 @@ int main(int argc, char** argv)
     // create a cost function
     Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
     Eigen::Matrix<double, control_dim, control_dim> R;
-    Q_final << 10000, 0, 0, 0, 10000, 0, 0, 0, 10000;
-    //Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    Q_final.setZero();
+    Q_final.diagonal() << 10000, 10000, 10000, 10000, 100000, 10000;
     Q.setZero();
-    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
-    ManifoldState_t x_final = manif::SO3<double>(0, 0, 0);
+    // Q.diagonal() << 3, 3, 3, 3, 3, 3;
+    R.setZero();
+    R.diagonal() << 1, 1, 1;
+    ManifoldState_t x_final;
+    x_final.pos() = manif::SO3<double>(0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
-    ManifoldState_t x_nominal = x0;
+    ManifoldState_t x_nominal = x_final;
     ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
     std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
         new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
@@ -156,19 +506,20 @@ int main(int argc, char** argv)
     x_curr = x0;
     x_traj.front() = x0;
     std::cout << "integrate an random initial state with the unstable system" << std::endl;
-    std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    std::cout << std::setprecision(4) << "m: " << x_curr << "\resulting_t tan: " << x_curr.log() << std::endl;
     for (size_t i = 0; i < N; i++)
     {
         exampleSystem->computeControlledDynamics(x_traj[i], 0, u_traj[i], dx);
-        x_curr = x_traj[i] + dx;
+        std::cout << "dx" << dx << std::endl;
+        x_curr = x_traj[i].rplus(dx);
         if (use_single_shooting)
             x_traj[i + 1] = x_curr;
         b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
         std::cout << "b: " << b[i] << std::endl;
-        std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        std::cout << std::setprecision(10) << "m: " << x_curr << "\resulting_t tan: " << x_curr.log() << std::endl;
     }
 
-    size_t nIter = 13;
+    size_t nIter = 25;
     for (size_t iter = 0; iter < nIter; iter++)
     {
         // initialize the optimal control problems for both solvers
@@ -183,7 +534,7 @@ int main(int argc, char** argv)
             for (size_t i = 0; i < N; i++)
             {
                 auto e = x_nominal.rminus(x_traj[i]);
-                // compute PT matrix w.r.t. current ref traj // TODO: clarifiy formulation of error in cost function
+                // compute PT matrix w.r.resulting_t. current ref traj // TODO: clarify formulation of error in cost function
                 auto e_adj = (e.exp()).adj();
                 problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] *
                                        e_adj.transpose();  // TODO: sort out that thing with the cost function
@@ -191,21 +542,21 @@ int main(int argc, char** argv)
             }
             // terminal stage transportation
             // TODO: clarifiy formulation of error in cost function
-            auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.t. current ref traj
-            auto e_adj = (e.exp()).adj();
+            // auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.resulting_t. current ref traj
+            // auto e_adj = (e.exp()).adj();
             //std::cout << "cost adj" << std::endl << e_adj << std::endl;
-            problems[idx]->Q_.back() = /*e_adj * */ problems[idx]->Q_.back();  // * e_adj.transpose();
-            problems[idx]->qv_.back() = /*e_adj * */ problems[idx]->qv_.back();
+            //problems[idx]->Q_.back() = e_adj * problems[idx]->Q_.back() * e_adj.transpose();
+            //problems[idx]->qv_.back() = e_adj * problems[idx]->qv_.back();
+            //std::cout << "problems[idx]->Q_.back()" << std::endl << problems[idx]->Q_.back() << std::endl;
+            //std::cout << "problems[idx]->qv_.back()" << std::endl << problems[idx]->qv_.back() << std::endl;
 
             // dynamics transportation
             for (size_t i = 0; i < N; i++)
             {
                 // std::cout << "dyn transport matrices" << std::endl;
-                Eigen::Matrix3d Jl, Jr;
-                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);
+                //Eigen::Matrix3d Jl, Jr;
+                auto l = x_traj[i + 1].rminus(x_traj[i]);
                 auto l_adj = (l.exp()).adj();
-                //std::cout << "l_adj" << std::endl << l_adj << std::endl;
-                //std::cout << std::endl;
                 problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
 
                 if (idx == 0)  // make the corrections for the standard riccati solver
@@ -243,40 +594,40 @@ int main(int argc, char** argv)
         ct::core::ControlVectorArray<control_dim> lv_sol_aug_riccati = lqocSolvers[1]->get_lv();
 
         // compare the quantities
-        for (size_t i = 0; i < lv_sol_riccati.size(); i++)
-        {
-            if (lv_sol_riccati[i].isApprox(lv_sol_aug_riccati[i], 1e-6) == false)
-            {
-                std::cout << lv_sol_riccati[i].transpose() << std::endl;
-                std::cout << lv_sol_aug_riccati[i].transpose() << std::endl;
-                throw std::runtime_error("lv solutions do not match");
-            }
-        }
-        for (size_t i = 0; i < KSol_riccati.size(); i++)
-        {
-            if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
-                throw std::runtime_error("K solutions do not match");
-        }
-
-        for (size_t i = 0; i < uSol_riccati.size(); i++)
-        {
-            if (uSol_riccati[i].isApprox(uSol_aug_riccati[i], 1e-6) == false)
-            {
-                std::cout << "for index " << i << std::endl;
-                std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
-                std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
-                throw std::runtime_error("u solutions do not match");
-            }
-        }
-        for (size_t i = 0; i < xSol_riccati.size(); i++)
-        {
-            if (xSol_riccati[i].isApprox(xSol_aug_riccati[i], 1e-6) == false)
-            {
-                std::cout << xSol_riccati[i].transpose() << std::endl;
-                std::cout << xSol_aug_riccati[i].transpose() << std::endl;
-                throw std::runtime_error("x solutions do not match");
-            }
-        }
+        //for (size_t i = 0; i < lv_sol_riccati.size(); i++)
+        //{
+        //    if ((lv_sol_riccati[i] - lv_sol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << std::setprecision(10) << lv_sol_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << lv_sol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("lv solutions do not match");
+        //    }
+        //}
+        //for (size_t i = 0; i < KSol_riccati.size(); i++)
+        //{
+        //    if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
+        //        throw std::runtime_error("K solutions do not match");
+        //}
+        //
+        //for (size_t i = 0; i < uSol_riccati.size(); i++)
+        //{
+        //    if ((uSol_riccati[i] - uSol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << "for index " << i << std::endl;
+        //        std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("u solutions do not match");
+        //    }
+        //}
+        //for (size_t i = 0; i < xSol_riccati.size(); i++)
+        //{
+        //    if ((xSol_riccati[i] - xSol_aug_riccati[i]).coeffs().array().abs().maxCoeff() > 1e-8)
+        //    {
+        //        std::cout << xSol_riccati[i].transpose() << std::endl;
+        //        std::cout << xSol_aug_riccati[i].transpose() << std::endl;
+        //        throw std::runtime_error("x solutions do not match");
+        //    }
+        //}
 
         //std::cout << std::setprecision(4) << "dx solution from riccati solver and directly added state_traj, iter "
         //          << iter << std::endl;
@@ -293,39 +644,60 @@ int main(int argc, char** argv)
         //            x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
         //        angularDiff = old_rot.angularDistance(new_rot);
         //    }
-        //    std::cout << "m:" << x_solver_direct[j] << "\t dx:" << xSol_riccati[j].transpose()
-        //              << "\t -- rot diff norm(): " << angularDiff << std::endl;
+        //    std::cout << "m:" << x_solver_direct[j] << "\resulting_t dx:" << xSol_riccati[j].transpose()
+        //              << "\resulting_t -- rot diff norm(): " << angularDiff << std::endl;
         //}
 
         std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
         x_curr = x0;
         std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
         ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
+        double d_cum_sum = 0;
+        double dx_cum_sum = 0;
+        double cost_sum = 0;
         for (size_t i = 0; i < N; i++)
         {
-            ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
-            // TODO: some term is missing here;
+            dx.setZero();
+            //Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
+
             if (use_single_shooting)
+            {
+                // TODO: some term is missing here;
+                ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
                 u_traj[i] += lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
-            else
+                exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
+                x_traj[i + 1] = x_traj[i] + dx;
+            }
+            else  // multiple shooting
+            {
                 u_traj[i] += uSol_riccati[i];
+                x_traj[i + 1] = x_traj[i + 1].rplus(xSol_riccati[i + 1]);
+                exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
+            }
 
-            exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
-            Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
-
-            if (use_single_shooting)
-                x_traj[i + 1] = x_traj[i] + dx;
-            else
-                x_traj[i + 1] = x_traj_prev[i + 1] + xSol_riccati[i + 1];
+            //Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
+            //std::cout << "m: " << x_traj[i + 1] << "\resulting_t log: " << x_traj[i + 1].log() << " u: " << u_traj[i].transpose()
+            //          << std::endl;
 
-            Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
-            std::cout << "m: " << x_traj[i + 1] << "\t tan: " << x_traj[i + 1].log()
-                      << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
+            // compute defect
+            b[i] = dx - (x_traj[i + 1].rminus(x_traj[i]));
 
-            b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
+            // compute update norms
+            d_cum_sum += b[i].coeffs().norm();
+            dx_cum_sum += xSol_riccati[i + 1].coeffs().norm();
+            //std::cout << "b: " << b[i] << std::endl;
 
-            std::cout << "b: " << b[i] << std::endl;
+            // compute running cost
+            costFunction->setCurrentStateAndControl(x_traj[i], u_traj[i], i * dt);
+            cost_sum += costFunction->evaluateIntermediate();
         }
+
+        // compute terminal cost
+        costFunction->setCurrentStateAndControl(x_traj.back(), u_traj.back(), N * dt);
+        cost_sum += costFunction->evaluateTerminal();
+
+        std::cout << std::setprecision(10) << "d_norm: \t " << d_cum_sum << "\t dx_norm: \t" << dx_cum_sum
+                  << " \t Jcost: " << cost_sum << std::endl;
         std::cout << std::endl << std::endl;
     }  // end iter
     return 1;

From 5779b6087317a21b3d77dcf8d0f2e3651d911496 Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Tue, 12 May 2020 20:41:06 +0200
Subject: [PATCH 05/21] completed working example of LQOC (multiple-shooting)
 on composite manifold S3xR3

---
 .../CostFunctionQuadraticSimple-impl.hpp      |  48 ++-
 .../CostFunctionQuadraticSimple.hpp           |   4 +-
 .../ct/optcon/problem/LQOCProblem-impl.hpp    |   2 +-
 .../solver/lqp/AugGNRiccatiSolver-impl.hpp    | 311 ----------------
 .../optcon/solver/lqp/AugGNRiccatiSolver.hpp  | 113 ------
 .../solver/lqp/GNRiccatiSolver-impl.hpp       |   2 +-
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 350 ++++--------------
 7 files changed, 106 insertions(+), 724 deletions(-)
 delete mode 100644 ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
 delete mode 100644 ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp

diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index 2d516c791..0eb40671a 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -11,16 +11,16 @@ namespace optcon {
 template <typename MANIFOLD, size_t CONTROL_DIM>
 CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple()
 {
+    x_deviation_.setZero();
     x_nominal_.setZero();
     Q_.setZero();
+    Jl_.setIdentity();
+    Jr_.setIdentity();
     u_nominal_.setZero();
     R_.setZero();
     x_final_.setZero();
     Q_final_.setZero();
-    x_deviation_.setZero();
     u_deviation_.setZero();
-    J_curr_.setIdentity();
-    J_ref_.setIdentity();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -34,8 +34,8 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
 {
     x_deviation_.setZero();
     u_deviation_.setZero();
-    J_curr_.setIdentity();
-    J_ref_.setIdentity();
+    Jl_.setIdentity();
+    Jr_.setIdentity();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -46,10 +46,10 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::~CostFunctionQuadraticSimple
 template <typename MANIFOLD, size_t CONTROL_DIM>
 CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(const CostFunctionQuadraticSimple& arg)
     : x_deviation_(arg.x_deviation_),
-      J_curr_(arg.J_curr_),
-      J_ref_(arg.J_ref_),
       x_nominal_(arg.x_nominal_),
       Q_(arg.Q_),
+      Jl_(arg.Jl_),
+      Jr_(arg.Jr_),
       u_deviation_(arg.u_deviation_),
       u_nominal_(arg.u_nominal_),
       R_(arg.R_),
@@ -74,15 +74,14 @@ void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::setCurrentStateAndContr
     this->t_ = t;
 
     this->u_deviation_ = u - u_nominal_;
-
-    //this->x_deviation_ = x - x_nominal_;
-    this->x_deviation_ = x_nominal_.rminus(this->x_, J_ref_, J_curr_);
+    this->x_deviation_ = x_nominal_.rminus(this->x_, Jl_, Jr_);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate() -> SCALAR
 {
-    SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Q_ * x_deviation_)(0);
+    // TODO: including Jr_ might not be required here (identical result)
+    SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Jr_ * Q_ * Jr_.transpose() * x_deviation_)(0);
     SCALAR costR = SCALAR(0.5) * (u_deviation_.transpose() * R_ * u_deviation_)(0);
     return costQ + costR;
 }
@@ -90,13 +89,13 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate()
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
 {
-    return Q_ * J_curr_.transpose() * x_deviation_;
+    return Q_ * Jr_.transpose() * x_deviation_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeIntermediate() -> state_matrix_t
 {
-    return J_curr_ * Q_ * J_curr_.transpose();
+    return Jr_ * Q_ * Jr_.transpose();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -120,29 +119,26 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateControlDerivativeI
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
 {
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
-    return SCALAR(0.5) * (x_deviation_final.transpose() * Q_final_ * x_deviation_final)(0);
+    // TODO: including Jr_ might not be required here (identical result)
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    return SCALAR(0.5) * (x_deviation_final.transpose() * Jr * Q_final_ * Jr.transpose() * x_deviation_final)(0);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
 {
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr, J_ref;
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
-    return Q_final_ * J_curr.transpose() * x_deviation_final;
-    //return - Q_final_  * x_deviation_final;
-    //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    return Q_final_ * Jr.transpose() * x_deviation_final;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr, J_ref;
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, J_ref, J_curr);
-    return J_curr * Q_final_ * J_curr.transpose();
-    //return Q_final_ ;
-    //typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, J_ref, J_curr);
-    //return Q_final_;
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    return Jr * Q_final_ * Jr.transpose();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
index 5d1278cd0..a912963b7 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -91,8 +91,8 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<MANIFOLD, CONTR
     typename MANIFOLD::Tangent x_deviation_;
     MANIFOLD x_nominal_;
     state_matrix_t Q_;
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_curr_; // TODO: rename
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> J_ref_; // TODO: rename
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl_; // TODO: get rid of Jl cleanly
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jr_;
 
     control_vector_t u_deviation_;
     control_vector_t u_nominal_;
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
index c61b8f205..a9f832552 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -319,7 +319,7 @@ void LQOCProblem<MANIFOLD, CONTROL_DIM>::setFromTimeInvariantLinearQuadraticProb
 {
     setZero();
 
-    if (b.size() != K_ + 1)
+    if ((int)b.size() != K_ + 1)
         throw std::runtime_error("b size not correct.");
 
     // get LTI dynamics
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
deleted file mode 100644
index a0796490e..000000000
--- a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp
+++ /dev/null
@@ -1,311 +0,0 @@
-/**********************************************************************************************************************
-This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
-Licensed under the BSD-2 license (see LICENSE file in main directory)
- **********************************************************************************************************************/
-
-#pragma once
-
-namespace ct {
-namespace optcon {
-
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::AugGNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
-    : LQOCSolver<MANIFOLD, CONTROL_DIM>(lqocProblem), N_(-1)
-{
-    Eigen::initParallel();
-    Eigen::setNbThreads(settings_.nThreadsEigen);
-}
-
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::AugGNRiccatiSolver(int N)
-{
-    changeNumberOfStages(N);
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solve()
-{
-    for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
-        solveSingleStage(i);
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int n)
-{
-    if (n == this->lqocProblem_->getNumberOfStages() - 1)
-        initializeCostToGo();
-
-    designController(n);
-
-    if (n > 0)
-        computeCostToGo(n);
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::configure(const NLOptConSettings& settings)
-{
-    settings_ = settings;
-    H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
-{
-    LQOCProblem_t& p = *this->lqocProblem_;
-
-    this->x_sol_[0].setZero();  // should always be zero (fixed init state)
-
-    for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
-    {
-        //! control update rule in diff coordinates
-        this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
-
-        //! state update rule in diff coordinates
-        // Note that we need to transport the state update into the tagent space of k+1
-        this->x_sol_[k + 1] =
-            p.Adj_x_[k + 1].transpose() * (p.A_[k] * this->x_sol_[k] + p.B_[k] * this->u_sol_[k] + p.b_[k]);
-    }
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeFeedbackMatrices()
-{ /*no action required, already computed in backward pass */
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::compute_lv()
-{ /*no action required, already computed in backward pass*/
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-auto AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::getSmallestEigenvalue() -> SCALAR
-{
-    return smallestEigenvalue_;
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
-{
-    if (lqocProblem->isConstrained())
-    {
-        throw std::runtime_error(
-            "Selected wrong solver - AugGNRiccatiSolver cannot handle constrained problems. Use a different solver");
-    }
-
-    const int& N = lqocProblem->getNumberOfStages();
-    changeNumberOfStages(N);
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::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);
-
-    this->lv_.resize(N);
-    this->L_.resize(N);
-
-    this->x_sol_.resize(N + 1);
-    this->u_sol_.resize(N);
-
-    sv_.resize(N + 1);
-    S_.resize(N + 1);
-    sv_t_.resize(N + 1);
-    S_t_.resize(N + 1);
-
-    N_ = N;
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::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];
-
-    if (ct::core::is_euclidean<MANIFOLD>::value)  // euclidean case
-    {
-        S_t_[N] = S_[N];
-        sv_t_[N] = sv_[N];
-    }
-    else
-    {  // manifold case requires parallel transport of value function
-        S_t_[N] = p.Adj_x_[N] * S_[N] * p.Adj_x_[N].transpose();
-        sv_t_[N] = p.Adj_x_[N] * sv_[N];
-    }
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
-{
-    LQOCProblem_t& p = *this->lqocProblem_;
-
-    S_[k] = p.Q_[k];
-    S_[k]/*.noalias()*/ += p.A_[k].transpose() * S_t_[k + 1] * p.A_[k]; // TODO: bring back all the noalias()
-    S_[k]/*.noalias()*/ -= this->L_[k].transpose() * Hi_[k] * this->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_t_[k + 1];
-    sv_[k]/*.noalias()*/ += p.A_[k].transpose() * S_t_[k + 1] * p.b_[k];
-    sv_[k]/*.noalias()*/ += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
-    sv_[k]/*.noalias()*/ += this->L_[k].transpose() * gv_[k];
-    sv_[k]/*.noalias()*/ += G_[k].transpose() * this->lv_[k];
-
-    if (ct::core::is_euclidean<MANIFOLD>::value)  // euclidean case
-    {
-        S_t_[k] = S_[k];
-        sv_t_[k] = sv_[k];
-    }
-    else
-    {  // manifold case requires parallel transport of value function to previous stage
-        S_t_[k] = p.Adj_x_[k] * S_[k] * p.Adj_x_[k].transpose();
-        sv_t_[k] = p.Adj_x_[k] * sv_[k];
-    }
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
-{
-    LQOCProblem_t& p = *this->lqocProblem_;
-
-    gv_[k] = p.rv_[k];
-    gv_[k].noalias() += p.B_[k].transpose() * sv_t_[k + 1];
-    gv_[k].noalias() += p.B_[k].transpose() * S_t_[k + 1].template selfadjointView<Eigen::Lower>() * p.b_[k];
-
-    G_[k] = p.P_[k];  // TODO: G_ could be left away for the first stage (efficiency)
-    //G_[k].noalias() += B_[k].transpose() * S_[k+1] * A_[k];
-    G_[k].noalias() += p.B_[k].transpose() * S_t_[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_t_[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 // TODO: this could be left away for the first stage (efficiency)
-        this->L_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * G_[k];
-
-        // calculate FF update
-        this->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
-        this->L_[k].noalias() = Hi_inverse_[k] * G_[k];
-
-        // calculate FF update
-        this->lv_[k].noalias() = Hi_inverse_[k] * gv_[k];
-    }
-}
-
-template <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::logToMatlab()
-{
-#ifdef MATLAB_FULL_LOG
-
-    matFile_.open("AugGNRiccatiSolver.mat");
-
-    matFile_.put("sv", sv_.toImplementation());
-    matFile_.put("S", S_.toImplementation());
-    matFile_.put("L", this->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 <typename MANIFOLD, size_t CONTROL_DIM>
-void AugGNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeAndAllocate()
-{
-    // do nothing
-}
-
-}  // namespace optcon
-}  // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
deleted file mode 100644
index 2bf54c7ef..000000000
--- a/ct_optcon/include/ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/**********************************************************************************************************************
-This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
-Licensed under the BSD-2 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 <typename MANIFOLD, size_t CONTROL_DIM>
-class AugGNRiccatiSolver : public LQOCSolver<MANIFOLD, CONTROL_DIM>
-{
-public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    static const int STATE_DIM = MANIFOLD::TangentDim;
-    static const int state_dim = STATE_DIM;
-    static const int control_dim = CONTROL_DIM;
-
-    using Base = LQOCSolver<MANIFOLD, CONTROL_DIM>;
-    using SCALAR = typename Base::SCALAR;
-    typedef typename Base::LQOCProblem_t 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::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
-
-    AugGNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr);
-
-    AugGNRiccatiSolver(int N);
-
-    virtual void solve() override;
-
-    virtual void initializeAndAllocate() override;
-
-    virtual void solveSingleStage(int N) override;
-
-    virtual void configure(const NLOptConSettings& settings) override;
-
-    virtual void computeStatesAndControls() override;
-
-    virtual void computeFeedbackMatrices() override;
-
-    virtual void compute_lv() 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_;
-
-    //! terms for quadratic value function
-    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_;
-    StateMatrixArray S_;
-    //! parallel-transported terms of value function
-    ct::core::DiscreteArray<typename MANIFOLD::Tangent> sv_t_;
-    StateMatrixArray S_t_;
-
-    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/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index ea9748b63..f6df146e5 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -63,7 +63,7 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backward pass"
+        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backward pass" //TODO: document this
         StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backward pass"
         typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backward pass"
         // Note that we need to transport the state update into the tagent space of k+1
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index e7dd2db10..4ecf41b5a 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -1,8 +1,5 @@
 
 #include <ct/optcon/optcon.h>
-#include <ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp>
-#include <ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp>
-
 
 using namespace ct::core;
 using namespace ct::optcon;
@@ -10,14 +7,16 @@ const bool verbose = true;
 
 
 template <typename POS_MAN, typename VEL_MAN, typename POS_TANGENT, typename VEL_TANGENT>
-class CompositeManifold;  // forward declaratoin
+class CompositeManifold;  // forward declaration, do not delete
 
 template <typename POS_MAN, typename VEL_MAN, typename POS_TANGENT, typename VEL_TANGENT>
 class CompositeManifoldTangent
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
     static constexpr size_t DoF = POS_TANGENT::DoF + VEL_TANGENT::DoF;
+
     using Scalar = double;
     using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
     using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
@@ -35,8 +34,8 @@ class CompositeManifoldTangent
         // TODO a dimension assert would not hurt
     }
 
-    CompositeManifoldTangent(const CompositeManifoldTangent& resulting_t)
-        : t_pos_(resulting_t.t_pos_), t_vel_(resulting_t.t_vel_), coeffs_(resulting_t.coeffs_)
+    CompositeManifoldTangent(const CompositeManifoldTangent& arg)
+        : t_pos_(arg.t_pos_), t_vel_(arg.t_vel_), coeffs_(arg.coeffs_)
     {
     }
 
@@ -100,10 +99,9 @@ class CompositeManifoldTangent
 
     CompositeManifold<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT> exp(OptJacobianRef J_m_t = OptJacobianRef{}) const
     {
-        //std::cout << "now calling exp..." << std::endl;
         if (J_m_t)
         {
-            throw std::runtime_error("J-m-resulting_t in tangent exp not impl yet.");
+            throw std::runtime_error("J-m-t in tangent exp not impl yet.");
         }
 
         CompositeManifold<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT> c(
@@ -134,31 +132,31 @@ class CompositeManifoldTangent
 };
 
 template <typename _Stream, typename t1, typename t2, typename t3, typename t4>
-_Stream& operator<<(_Stream& s, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+_Stream& operator<<(_Stream& s, const CompositeManifoldTangent<t1, t2, t3, t4>& t)
 {
-    s << resulting_t.coeffs().transpose();
+    s << t.coeffs().transpose();
     return s;
 }
 
 template <class _EigenDerived, typename t1, typename t2, typename t3, typename t4>
-auto operator*(const Eigen::MatrixBase<_EigenDerived>& J, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+auto operator*(const Eigen::MatrixBase<_EigenDerived>& J, const CompositeManifoldTangent<t1, t2, t3, t4>& t)
 {
-    return J * resulting_t.coeffs();
+    return J * t.coeffs();
 }
 
 template <typename t1, typename t2, typename t3, typename t4>
-auto operator*(const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t, const double& s)
+auto operator*(const CompositeManifoldTangent<t1, t2, t3, t4>& t, const double& s)
 {
-    return resulting_t.coeffs() * s;
+    return t.coeffs() * s;
 }
 
 template <class _EigenDerived, typename t1, typename t2, typename t3, typename t4>
-auto operator+(const Eigen::MatrixBase<_EigenDerived>& tb, const CompositeManifoldTangent<t1, t2, t3, t4>& resulting_t)
+auto operator+(const Eigen::MatrixBase<_EigenDerived>& tb, const CompositeManifoldTangent<t1, t2, t3, t4>& t)
 {
     CompositeManifoldTangent<t1, t2, t3, t4> res;
     CompositeManifoldTangent<t1, t2, t3, t4> rhs(tb);
-    res.set_pos(resulting_t.pos() + rhs.pos());
-    res.set_vel(resulting_t.vel() + rhs.vel());
+    res.set_pos(t.pos() + rhs.pos());
+    res.set_vel(t.vel() + rhs.vel());
     return res;
 }
 
@@ -171,7 +169,7 @@ class CompositeManifold
 
     using Tangent = CompositeManifoldTangent<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT>;
     static constexpr size_t TangentDim = Tangent::DoF;
-    static constexpr size_t Dim = POS_MAN::Dim + VEL_MAN::Dim + 1;  // todo: this is hacky
+    static constexpr size_t Dim = POS_MAN::Dim + VEL_MAN::Dim + 1;  // TODO: this is hacky
 
     using Scalar = double;
     using Jacobian = Eigen::Matrix<Scalar, TangentDim, TangentDim>;
@@ -193,17 +191,17 @@ class CompositeManifold
         m_vel_.setIdentity();
     }
 
-    const Eigen::Matrix<Scalar, Dim, 1> coeffs() const  // todo: return by value is bad
+    const Eigen::Matrix<Scalar, Dim, 1> coeffs() const  // TODO: return by value is bad
     {
         Eigen::Matrix<Scalar, Dim, 1> c;
         c << m_pos_.coeffs(), m_vel_.coeffs();
         return c;
     }
 
-    CompositeManifold operator+(const Tangent& resulting_t) const { return rplus(resulting_t); }
-    //CompositeManifold operator+=(const Tangent& resulting_t)
+    CompositeManifold operator+(const Tangent& t) const { return rplus(t); }
+    //CompositeManifold operator+=(const Tangent& t)
     //{
-    //    *this = this->rplus(resulting_t);
+    //    *this = this->rplus(t);
     //    return *this;  // todo test
     //}
 
@@ -212,19 +210,19 @@ class CompositeManifold
      * NOTE: the operator also has to parallel transport velocities to the new tangent plane.
      * Since for rplus, the rhs is expressed in the tangent space of the lhs, we choose: "first add, then transport"
      */
-    CompositeManifold rplus(const Tangent& resulting_t, OptJacobianRef Jl = {}, OptJacobianRef Jr = {}) const
+    CompositeManifold rplus(const Tangent& t, OptJacobianRef Jl = {}, OptJacobianRef Jr = {}) const
     {
         typename POS_MAN::Jacobian pJl, pJr;
         typename VEL_MAN::Jacobian vJl, vJr;
 
-        VEL_MAN v_new_local = vel().rplus(resulting_t.vel(), vJl, vJr);
+        VEL_MAN v_new_local = vel().rplus(t.vel(), vJl, vJr);
 
-        VEL_MAN v_new_transported(resulting_t.pos().exp().adj().transpose() * v_new_local.coeffs());
+        VEL_MAN v_new_transported(t.pos().exp().adj().transpose() * v_new_local.coeffs());
 
         CompositeManifold c;
         c.vel() = v_new_transported;
-        //c.vel() = c.vel().rplus(resulting_t.vel(), vJl, vJr);
-        c.pos() = pos().rplus(resulting_t.pos(), pJl, pJr);  // todo make jacobians optional
+        //c.vel() = c.vel().rplus(t.vel(), vJl, vJr);
+        c.pos() = pos().rplus(t.pos(), pJl, pJr);  // todo make jacobians optional
 
         if (Jl)
         {
@@ -255,12 +253,12 @@ class CompositeManifold
         typename POS_MAN::Jacobian pJl, pJr;
         typename VEL_MAN::Jacobian vJl, vJr;
 
-        Tangent resulting_t;
-        resulting_t.set_pos(pos().rminus(rhs.pos(), pJl, pJr));  // todo make jacobians optional
-        VEL_MAN v_lhs_transported(resulting_t.pos().exp().adj() * vel().coeffs());
+        Tangent t;
+        t.set_pos(pos().rminus(rhs.pos(), pJl, pJr));  // todo make jacobians optional
+        VEL_MAN v_lhs_transported(t.pos().exp().adj() * vel().coeffs());
 
         VEL_TANGENT v_new_local = v_lhs_transported.rminus(rhs.vel(), vJl, vJr);
-        resulting_t.set_vel(v_new_local);
+        t.set_vel(v_new_local);
 
         if (Jl)
         {
@@ -276,7 +274,7 @@ class CompositeManifold
             (*Jr).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = vJr;
         }
 
-        return resulting_t;
+        return t;
     }
 
 
@@ -286,23 +284,13 @@ class CompositeManifold
             throw std::runtime_error("J_t_m compuation not defined.");
 
         return rminus(CompositeManifold::Identity(), J_t_m);
-        //resulting_t.set_pos(m_pos_.log());
-        //resulting_t.set_vel(m_vel_.log());
-        //return resulting_t;
     }
 
-    /**
-     * @brief The question
-     * The fundamental question remains: what is the velocity adjoint?
-     *  - multiple shooting: in the forward update rule, the computed velocities want to be transported to the next states manifold coordinate system
-     *  - single shooting: seems to be much more stable when velocity part adjoint is identity -- is the value function more stable???
-     */
     Jacobian adj() const
     {
         Jacobian J = Jacobian::Zero();
         J.template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = m_pos_.adj();
-        J.template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() =
-            m_pos_.adj();  //.setIdentity();  // = m_pos_.adj(); // TODO: what exactly is it?
+        J.template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = m_pos_.adj();
         return J;
     }
 
@@ -345,88 +333,23 @@ class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldSt
 
     virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(*this); }
     /**
-     * @brief the log operator is defined as expressing the tangent vector w.r.resulting_t. m_ref_
+     * @brief the log operator is defined as expressing the tangent vector w.r.t. the current position trajectory
      */
     virtual ManifoldState_t::Tangent lift(const ManifoldState_t& m) override
     {
-        ManifoldState_t::Tangent resulting_t = ManifoldState_t::Tangent::Zero();
-        //resulting_t.set_pos( "zero...");
-        resulting_t.set_vel(m.vel().log());
-        return resulting_t;
+        ManifoldState_t::Tangent t = ManifoldState_t::Tangent::Zero();
+        // t.set_pos() = ZERO // important!
+        t.set_vel(m.vel().log());
+        return t;
     }
 
-    //virtual ManifoldState_t retract(const ManifoldState_t::Tangent& resulting_t) override {
-    //     throw std::runtime_error(" retract not implemetned.");
-    //     /*return m_ref_.rplus(resulting_t);*/ }
 protected:
-    //ManifoldState_t m_ref_;
     double dt_;
 };
 
 
-// TODO: make this a unit test
-//void testParallelTransport()
-//{
-//    auto origin = manif::SE2<double>(0, 0, 0);
-//    Eigen::Matrix3d b;
-//    b.setIdentity();  // some basis, assumed to be expressed in the manifolds over which we iterate
-//
-//    for (double theta = M_PI / 2; theta <= 2 * M_PI; theta += M_PI / 2)
-//    {
-//        std::cout << "case theta = " << theta << std::endl;
-//        auto m = manif::SE2<double>(0, 0, theta);
-//        Eigen::Matrix3d Jl, Jr;  // jacobians
-//        auto d_rminus = m.rminus(origin, Jl, Jr);
-//        auto adj_rminus = d_rminus.exp().adj();  // the adjoint for transportation to the origin
-//
-//        auto d_between = m.between(origin);
-//        auto adj_between = d_rminus.exp().adj();
-//
-//        // ASSERT_TRUE(adj_rminus.isApprox(adj_between));
-//
-//        std::cout << adj_rminus * b << std::endl << std::endl;   // express basis b in origin-manifold
-//        std::cout << adj_between * b << std::endl << std::endl;  // express basis b in origin-manifold
-//        // std::cout << Jr * Jl * b << std::endl << std::endl;  // TODO: how does the Jac relate to the adjoint here?
-//        std::cout << std::endl;
-//    }
-//
-//    // auto m_pi_2 = manif::SE2<double>(0, 0, M_PI / 2);
-//    // auto m_pi = manif::SE2<double>(0, 0, M_PI);
-//    // auto m_mpi_2 = manif::SE2<double>(0, 0, -M_PI / 2);
-//    //
-//    //
-//    // std::cout << m_0.adj() * b << std::endl << std::endl;
-//    // std::cout << m_pi_2.adj() * b << std::endl << std::endl;
-//    // std::cout << m_pi.adj() * b << std::endl << std::endl;
-//    // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
-//}
-
 int main(int argc, char** argv)
 {
-    //ManifoldState_t x_test;
-    //x_test.setIdentity();
-    //Eigen::Vector3d vel_init(1.0, 0.0, 0.0);
-    //x_test.vel() = manif::R3d(vel_init);
-    ////x0.pos() = manif::SO3<double>(0, 0, M_PI);
-    //std::cout << "m: " << x_test << std::endl;
-    //ManifoldState_t::Tangent t_test;
-    //t_test.set_coeff(2, M_PI / 2);
-    //t_test.set_coeff(3, 1);
-    //
-    //std::cout << "resulting_t: " << t_test << std::endl;
-    //ManifoldState_t newX = x_test.rplus(t_test);
-    //std::cout << "newX: " << newX << std::endl;
-    //auto t_reverse = newX.rminus(x_test);
-    //std::cout << "t_reverse:" << t_reverse << std::endl;
-    //std::cout << "t_reverse.exp() " << t_reverse.exp() << std::endl;
-    //std::cout << "t_reverse.exp().log() " << t_reverse.exp().log() << std::endl;
-    //
-    //std::cout << "parallel transport study: " << std::endl;
-    //auto ADJ = t_reverse.exp().adj();
-    //std::cout << "adj: " << ADJ << std::endl;
-    //std::cout << "adj*t_test: " << ADJ * t_test << std::endl;
-    //exit(0);
-
     std::cout << std::fixed;
 
     const bool use_single_shooting = true;  // toggle between single and multiple shooting
@@ -436,7 +359,7 @@ int main(int argc, char** argv)
 
     ManifoldState_t x0;
     x0.setIdentity();
-    x0.pos() = manif::SO3<double>(M_PI/2, M_PI/2, 0.0);
+    x0.pos() = manif::SO3<double>(M_PI / 2, M_PI / 2, 0.0);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
     ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
         N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
@@ -454,27 +377,12 @@ int main(int argc, char** argv)
     }
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
-        new GNRiccatiSolver<ManifoldState_t, control_dim>);
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> augGnRiccatiSolver(
-        new AugGNRiccatiSolver<ManifoldState_t, control_dim>);
-
-    // store them, and identifying names, in a vectors
-    std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
-    lqocSolvers.push_back(gnRiccatiSolver);
-    lqocSolvers.push_back(augGnRiccatiSolver);
-    std::vector<std::string> solverNames = {"Riccati", "AugRiccati"};
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> solver(new GNRiccatiSolver<ManifoldState_t, control_dim>);
 
     // create linear-quadratic optimal control problem containers
-    std::vector<std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>>> problems;
-    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem1(
-        new LQOCProblem<ManifoldState_t, control_dim>(N));
-    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> lqocProblem2(
+    std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> problem(
         new LQOCProblem<ManifoldState_t, control_dim>(N));
 
-    problems.push_back(lqocProblem1);
-    problems.push_back(lqocProblem2);
-
     // create a discrete-time manifold system
     std::shared_ptr<ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
         new DiscrSO3LTITestSystem(dt));
@@ -488,7 +396,7 @@ int main(int argc, char** argv)
     Q_final.setZero();
     Q_final.diagonal() << 10000, 10000, 10000, 10000, 100000, 10000;
     Q.setZero();
-    // Q.diagonal() << 3, 3, 3, 3, 3, 3;
+    Q.diagonal() << 3, 3, 3, 3, 3, 3;
     R.setZero();
     R.diagonal() << 1, 1, 1;
     ManifoldState_t x_final;
@@ -496,6 +404,8 @@ int main(int argc, char** argv)
     std::cout << "desired final state: " << x_final << std::endl;
     ManifoldState_t x_nominal = x_final;
     ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
+
+    // TODO: this currently only works with CostFunctionQuadratic simple (others not ported yet)
     std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
         new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
 
@@ -506,151 +416,59 @@ int main(int argc, char** argv)
     x_curr = x0;
     x_traj.front() = x0;
     std::cout << "integrate an random initial state with the unstable system" << std::endl;
-    std::cout << std::setprecision(4) << "m: " << x_curr << "\resulting_t tan: " << x_curr.log() << std::endl;
+    //std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     for (size_t i = 0; i < N; i++)
     {
         exampleSystem->computeControlledDynamics(x_traj[i], 0, u_traj[i], dx);
-        std::cout << "dx" << dx << std::endl;
+        //std::cout << "dx" << dx << std::endl;
         x_curr = x_traj[i].rplus(dx);
         if (use_single_shooting)
             x_traj[i + 1] = x_curr;
         b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
-        std::cout << "b: " << b[i] << std::endl;
-        std::cout << std::setprecision(10) << "m: " << x_curr << "\resulting_t tan: " << x_curr.log() << std::endl;
+        //std::cout << "b: " << b[i] << std::endl;
+        //std::cout << std::setprecision(10) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
     size_t nIter = 25;
     for (size_t iter = 0; iter < nIter; iter++)
     {
-        // initialize the optimal control problems for both solvers
-        problems[0]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
-        problems[1]->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
+        // initialize the LQ optimal control problem
+        problem->setFromTimeInvariantLinearQuadraticProblem(x_traj, u_traj, *linearizer, *costFunction, b, dt);
 
-
-        // HACKY corrections // TODO: move somewhere meaningful
-        for (size_t idx : {0, 1})
+        // dynamics transportation for backwards riccati pass and forward solution candidate update
+        for (size_t i = 0; i < N; i++)
         {
-            // intermediate stages cost transportation
-            for (size_t i = 0; i < N; i++)
-            {
-                auto e = x_nominal.rminus(x_traj[i]);
-                // compute PT matrix w.r.resulting_t. current ref traj // TODO: clarify formulation of error in cost function
-                auto e_adj = (e.exp()).adj();
-                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] *
-                                       e_adj.transpose();  // TODO: sort out that thing with the cost function
-                problems[idx]->qv_[i] = e_adj * problems[idx]->qv_[i];
-            }
-            // terminal stage transportation
-            // TODO: clarifiy formulation of error in cost function
-            // auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.resulting_t. current ref traj
-            // auto e_adj = (e.exp()).adj();
-            //std::cout << "cost adj" << std::endl << e_adj << std::endl;
-            //problems[idx]->Q_.back() = e_adj * problems[idx]->Q_.back() * e_adj.transpose();
-            //problems[idx]->qv_.back() = e_adj * problems[idx]->qv_.back();
-            //std::cout << "problems[idx]->Q_.back()" << std::endl << problems[idx]->Q_.back() << std::endl;
-            //std::cout << "problems[idx]->qv_.back()" << std::endl << problems[idx]->qv_.back() << std::endl;
-
-            // dynamics transportation
-            for (size_t i = 0; i < N; i++)
-            {
-                // std::cout << "dyn transport matrices" << std::endl;
-                //Eigen::Matrix3d Jl, Jr;
-                auto l = x_traj[i + 1].rminus(x_traj[i]);
-                auto l_adj = (l.exp()).adj();
-                problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
-
-                if (idx == 0)  // make the corrections for the standard riccati solver
-                {
-                    problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
-                    problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
-                    problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
-                }
-            }
+            auto l = x_traj[i + 1].rminus(x_traj[i]);
+            auto l_adj = (l.exp()).adj();
+            problem->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
+
+            // make the necessary overloads, which allows the standard riccati solver to backpropagate on manifolds
+            problem->A_[i] = l_adj.transpose() * problem->A_[i];
+            problem->B_[i] = l_adj.transpose() * problem->B_[i];
+            problem->b_[i] = l_adj.transpose() * problem->b_[i];
+        }
 
-            // set the problem pointers
-            lqocSolvers[idx]->setProblem(problems[idx]);
+        // set the problem pointers
+        solver->setProblem(problem);
 
-            // allocate memory (if required)
-            lqocSolvers[idx]->initializeAndAllocate();
+        // allocate memory (if required)
+        solver->initializeAndAllocate();
 
-            // solve the problems...
-            lqocSolvers[idx]->solve();
+        // solve the problem
+        solver->solve();
 
-            // postprocess data
-            lqocSolvers[idx]->compute_lv();
-            lqocSolvers[idx]->computeFeedbackMatrices();
-            lqocSolvers[idx]->computeStatesAndControls();
-        }
+        // postprocess data
+        solver->compute_lv();
+        solver->computeFeedbackMatrices();
+        solver->computeStatesAndControls();
+
+        // retrieve solutions from solver
+        auto xSol = solver->getSolutionState();
+        auto uSol = solver->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol = solver->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol = solver->get_lv();
 
-        // retrieve solutions from both solvers
-        auto xSol_riccati = lqocSolvers[0]->getSolutionState();
-        auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
-
-        auto xSol_aug_riccati = lqocSolvers[1]->getSolutionState();
-        auto uSol_aug_riccati = lqocSolvers[1]->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol_aug_riccati = lqocSolvers[1]->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol_aug_riccati = lqocSolvers[1]->get_lv();
-
-        // compare the quantities
-        //for (size_t i = 0; i < lv_sol_riccati.size(); i++)
-        //{
-        //    if ((lv_sol_riccati[i] - lv_sol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
-        //    {
-        //        std::cout << std::setprecision(10) << lv_sol_riccati[i].transpose() << std::endl;
-        //        std::cout << std::setprecision(10) << lv_sol_aug_riccati[i].transpose() << std::endl;
-        //        throw std::runtime_error("lv solutions do not match");
-        //    }
-        //}
-        //for (size_t i = 0; i < KSol_riccati.size(); i++)
-        //{
-        //    if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
-        //        throw std::runtime_error("K solutions do not match");
-        //}
-        //
-        //for (size_t i = 0; i < uSol_riccati.size(); i++)
-        //{
-        //    if ((uSol_riccati[i] - uSol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
-        //    {
-        //        std::cout << "for index " << i << std::endl;
-        //        std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
-        //        std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
-        //        throw std::runtime_error("u solutions do not match");
-        //    }
-        //}
-        //for (size_t i = 0; i < xSol_riccati.size(); i++)
-        //{
-        //    if ((xSol_riccati[i] - xSol_aug_riccati[i]).coeffs().array().abs().maxCoeff() > 1e-8)
-        //    {
-        //        std::cout << xSol_riccati[i].transpose() << std::endl;
-        //        std::cout << xSol_aug_riccati[i].transpose() << std::endl;
-        //        throw std::runtime_error("x solutions do not match");
-        //    }
-        //}
-
-        //std::cout << std::setprecision(4) << "dx solution from riccati solver and directly added state_traj, iter "
-        //          << iter << std::endl;
-        //ct::core::DiscreteArray<ManifoldState_t> x_solver_direct(N + 1);
-        //for (size_t j = 0; j < xSol_riccati.size(); j++)
-        //{
-        //    x_solver_direct[j] = x_traj[j] + xSol_riccati[j];
-        //    double angularDiff = 0;
-        //    if (j > 0)
-        //    {
-        //        Eigen::Quaterniond old_rot(x_solver_direct[j - 1].w(), x_solver_direct[j - 1].x(),
-        //            x_solver_direct[j - 1].y(), x_solver_direct[j - 1].z());
-        //        Eigen::Quaterniond new_rot(
-        //            x_solver_direct[j].w(), x_solver_direct[j].x(), x_solver_direct[j].y(), x_solver_direct[j].z());
-        //        angularDiff = old_rot.angularDistance(new_rot);
-        //    }
-        //    std::cout << "m:" << x_solver_direct[j] << "\resulting_t dx:" << xSol_riccati[j].transpose()
-        //              << "\resulting_t -- rot diff norm(): " << angularDiff << std::endl;
-        //}
-
-        std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
         x_curr = x0;
-        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
         ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
         double d_cum_sum = 0;
         double dx_cum_sum = 0;
@@ -658,36 +476,29 @@ int main(int argc, char** argv)
         for (size_t i = 0; i < N; i++)
         {
             dx.setZero();
-            //Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
 
             if (use_single_shooting)
             {
-                // TODO: some term is missing here;
                 ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
-                u_traj[i] += lv_sol_riccati[i] + KSol_riccati[i] * (x_err /*- eucl. part here*/);
+                u_traj[i] += lv_sol[i] + KSol[i] * x_err;
                 exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
                 x_traj[i + 1] = x_traj[i] + dx;
             }
             else  // multiple shooting
             {
-                u_traj[i] += uSol_riccati[i];
-                x_traj[i + 1] = x_traj[i + 1].rplus(xSol_riccati[i + 1]);
+                u_traj[i] += uSol[i];
+                x_traj[i + 1] = x_traj[i + 1].rplus(xSol[i + 1]);
                 exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
             }
 
-            //Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
-            //std::cout << "m: " << x_traj[i + 1] << "\resulting_t log: " << x_traj[i + 1].log() << " u: " << u_traj[i].transpose()
-            //          << std::endl;
-
             // compute defect
             b[i] = dx - (x_traj[i + 1].rminus(x_traj[i]));
 
             // compute update norms
             d_cum_sum += b[i].coeffs().norm();
-            dx_cum_sum += xSol_riccati[i + 1].coeffs().norm();
-            //std::cout << "b: " << b[i] << std::endl;
+            dx_cum_sum += xSol[i + 1].coeffs().norm();
 
-            // compute running cost
+            // compute intermediate cost
             costFunction->setCurrentStateAndControl(x_traj[i], u_traj[i], i * dt);
             cost_sum += costFunction->evaluateIntermediate();
         }
@@ -698,7 +509,6 @@ int main(int argc, char** argv)
 
         std::cout << std::setprecision(10) << "d_norm: \t " << d_cum_sum << "\t dx_norm: \t" << dx_cum_sum
                   << " \t Jcost: " << cost_sum << std::endl;
-        std::cout << std::endl << std::endl;
     }  // end iter
     return 1;
 }
\ No newline at end of file

From 7048b4c8d1ac203792ff634a5ec812256a4722ff Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Mon, 18 May 2020 17:54:09 -0400
Subject: [PATCH 06/21] remove unneeded computation

L^T * gv + L^T * Hi * lv evaluates to zero
---
 ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 74021f1ef..8f90f0c9e 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -157,8 +157,6 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeCostToGo(size_t k)
     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() += this->L_[k].transpose() * Hi_[k] * this->lv_[k];
-    sv_[k].noalias() += this->L_[k].transpose() * gv_[k];
     sv_[k].noalias() += G_[k].transpose() * this->lv_[k];
 }
 

From f8d083d7ccb53cc5cdd445f9db7f732ce61b7c2c Mon Sep 17 00:00:00 2001
From: hmcm <hannah.markgraf@rwth-aachen.de>
Date: Wed, 20 May 2020 14:33:32 +0200
Subject: [PATCH 07/21] fixed bug concerning hpipm_status display

---
 ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
index c1f018ed0..f0abb7988 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -198,7 +198,7 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::solve()
         {
             printf("\n -> Solver failed! Minimum step length reached\n");
         }
-        else if (hpipm_status_ == 2)
+        else if (hpipm_status_ == 3)
         {
             printf("\n -> Solver failed! NaN in computations\n");
         }

From 9b9cedd227449cfa26bd7e25dc61615b755f5c28 Mon Sep 17 00:00:00 2001
From: Marco Frigerio <marco.frigerio@kuleuven.be>
Date: Wed, 20 May 2020 15:38:27 +0200
Subject: [PATCH 08/21] Fix links to RobCoGen in the readme

---
 README.md | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index fe586f98c..c08222b81 100644
--- a/README.md
+++ b/README.md
@@ -38,7 +38,7 @@ The CT was designed with the following features in mind:
  	- solve large scale optimal control problems in MPC fashion. 
   
  - **Robot Modelling, Rigid Body Kinematics and Dynamics**: 
-    - straight-forward interface to the state-of the art rigid body dynamics modelling tool RobCoGen.
+    - straight-forward interface to the state-of the art rigid body dynamics modelling tool [RobCoGen][robcogen-url].
 	- implementation of a basic nonlinear-programming inverse kinematics solver for fix-base robots.
  
  - **Automatic Differentiation**:
@@ -196,9 +196,10 @@ The four different main modules are detailed in the following.
  - Operational Space Controllers
  - Basic soft **auto-differentiable contact model** for arbitrary frames (ct::rbd::EEContactModel)
  - **Actuator dynamics** (ct::rbd::ActuatorDynamics)
- - Backend uses <a href="https://bitbucket.org/mfrigerio17/roboticscodegenerator/" target="_blank">RobCoGen</a> \cite frigerioCodeGen, 
+ - Backend uses [RobCoGen][robcogen-url], 
  a highly efficient Rigid Body Dynamics library
 
+[robcogen-url]: https://robcogenteam.bitbucket.io/
 
 ### ct_models
 

From cd7312038f1d62a29dbbfd23808bab65c6e79b7d Mon Sep 17 00:00:00 2001
From: Romain Reignier <rre@robopec.com>
Date: Fri, 22 May 2020 11:23:08 +0200
Subject: [PATCH 09/21] Downgrade minimum required CMake version to 3.5
 (xenial) and use newer Python Cmake helpers for Cmake > 3.12

---
 ct/CMakeLists.txt        |  2 +-
 ct_core/CMakeLists.txt   | 40 ++++++++++++++++++++++++++++------------
 ct_doc/CMakeLists.txt    |  2 +-
 ct_models/CMakeLists.txt |  2 +-
 ct_optcon/CMakeLists.txt |  2 +-
 ct_rbd/CMakeLists.txt    |  2 +-
 6 files changed, 33 insertions(+), 17 deletions(-)

diff --git a/ct/CMakeLists.txt b/ct/CMakeLists.txt
index 0df155c5c..8890c7609 100644
--- a/ct/CMakeLists.txt
+++ b/ct/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.14.7)
+cmake_minimum_required(VERSION 3.5)
 project(ct VERSION 3.0.2 LANGUAGES CXX)
 
 #Make sure metapackage does not fail when building documentation
diff --git a/ct_core/CMakeLists.txt b/ct_core/CMakeLists.txt
index 89840ce8f..00282ee0b 100755
--- a/ct_core/CMakeLists.txt
+++ b/ct_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
@@ -63,18 +63,30 @@ else()
     message(STATUS "COMPILING WITHOUT QWT")
 endif()
 
-find_package(Python COMPONENTS Development NumPy QUIET)
-if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3)
-   message(STATUS "Found python " ${Python_VERSION})
-   list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
-   set(PLOTTING_ENABLED ON)
-   message(STATUS "Python library path ... " ${Python_LIBRARIES})
-   list(APPEND ct_core_LIBS ${Python_LIBRARIES})
+if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+    find_package(PythonLibs 3 QUIET)
+    if (PYTHONLIBS_FOUND)
+        message(STATUS "Found python 3")
+        list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
+        set(PLOTTING_ENABLED ON)
+        message(STATUS "Python library path ... " ${PYTHON_LIBRARY})
+        list(APPEND ct_core_LIBS ${PYTHON_LIBRARY})
+    else()
+        message(STATUS "Python 3 not found, plotting will not be available")
+    endif()
 else()
-   message(STATUS "Python 3 not found, plotting will not be available")
+    find_package(Python COMPONENTS Development Numpy QUIET)
+    if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3)
+        message(STATUS "Found python " ${Python_VERSION})
+        list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
+        set(PLOTTING_ENABLED ON)
+        message(STATUS "Python library path ... " ${Python_LIBRARIES})
+        list(APPEND ct_core_LIBS ${Python_LIBRARIES})
+    else()
+        message(STATUS "Python 3 not found, plotting will not be available")
+    endif()
 endif()
 
-
 ## configure files required for code-generation
 set(CODEGEN_TEMPLATE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/templates")
 set(CODEGEN_OUTPUT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/generated")
@@ -87,8 +99,12 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ct/core/templateDir.h.in ${CM
 
 ## define the directories to be included in all ct_core targets
 list(APPEND ct_core_target_include_dirs ${EIGEN3_INCLUDE_DIR})
-list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS})
-list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS})
+if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
+    list(APPEND ct_core_target_include_dirs ${PYTHON_INCLUDE_DIRS})
+else()
+    list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS})
+    list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS})
+endif()
 list(APPEND ct_core_target_include_dirs ${QWT_INCLUDE_DIR})
 list(APPEND ct_core_target_include_dirs $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
 list(APPEND ct_core_target_include_dirs $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/examples/include>)
diff --git a/ct_doc/CMakeLists.txt b/ct_doc/CMakeLists.txt
index e764aeaeb..3305b9fc4 100755
--- a/ct_doc/CMakeLists.txt
+++ b/ct_doc/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 project (ct_doc VERSION 3.0.2 )
 
diff --git a/ct_models/CMakeLists.txt b/ct_models/CMakeLists.txt
index 1ee7b3755..3667c6d80 100755
--- a/ct_models/CMakeLists.txt
+++ b/ct_models/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
diff --git a/ct_optcon/CMakeLists.txt b/ct_optcon/CMakeLists.txt
index e53eab979..5ee87eb86 100644
--- a/ct_optcon/CMakeLists.txt
+++ b/ct_optcon/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
diff --git a/ct_rbd/CMakeLists.txt b/ct_rbd/CMakeLists.txt
index 62173b8cb..f333359b0 100644
--- a/ct_rbd/CMakeLists.txt
+++ b/ct_rbd/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required (VERSION 3.14.7)
+cmake_minimum_required (VERSION 3.5)
 
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)

From 600a62915a7383adf523102664652ff14f67100b Mon Sep 17 00:00:00 2001
From: Romain Reignier <rre@robopec.com>
Date: Fri, 22 May 2020 11:23:45 +0200
Subject: [PATCH 10/21] Fix histogram plot crash

The bins argument must be an integer
---
 ct_core/src/core/plot/plot.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ct_core/src/core/plot/plot.cpp b/ct_core/src/core/plot/plot.cpp
index d972902fd..1a52d174d 100644
--- a/ct_core/src/core/plot/plot.cpp
+++ b/ct_core/src/core/plot/plot.cpp
@@ -238,7 +238,7 @@ bool hist(const Eigen::Ref<const Eigen::VectorXd>& x, const double bins, const s
         PyList_SetItem(xlist, i, PyFloat_FromDouble(x(i)));
     }
 
-    PyObject* bins_py = PyFloat_FromDouble(bins);
+    PyObject* bins_py = PyLong_FromDouble(bins);
     PyObject* histtype_py = PyString_FromString(histtype.c_str());
 
     // construct positional args

From 6d6ea58269e13f29d727739ad1cc977e74f360f6 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Sat, 23 May 2020 10:32:01 +0200
Subject: [PATCH 11/21] Update ct_core/CMakeLists.txt with suggested commits

Co-authored-by: acxz <17132214+acxz@users.noreply.github.com>
---
 ct_core/CMakeLists.txt | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/ct_core/CMakeLists.txt b/ct_core/CMakeLists.txt
index 00282ee0b..80737cc0c 100755
--- a/ct_core/CMakeLists.txt
+++ b/ct_core/CMakeLists.txt
@@ -75,7 +75,7 @@ if(${CMAKE_VERSION} VERSION_LESS "3.12.0")
         message(STATUS "Python 3 not found, plotting will not be available")
     endif()
 else()
-    find_package(Python COMPONENTS Development Numpy QUIET)
+    find_package(Python COMPONENTS Development NumPy QUIET)
     if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3)
         message(STATUS "Found python " ${Python_VERSION})
         list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED)
@@ -214,4 +214,3 @@ endif()
 #################
 add_subdirectory(doc)
 
-

From f44104c59653fe9b4f0507356df0265dcac8046d Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Tue, 2 Jun 2020 15:32:19 -0400
Subject: [PATCH 12/21] remove repeated header install line

---
 ct_optcon/CMakeLists.txt | 2 --
 1 file changed, 2 deletions(-)

diff --git a/ct_optcon/CMakeLists.txt b/ct_optcon/CMakeLists.txt
index 5ee87eb86..9e9977565 100644
--- a/ct_optcon/CMakeLists.txt
+++ b/ct_optcon/CMakeLists.txt
@@ -183,8 +183,6 @@ endif()
 # for correct libraries locations across platforms
 include(GNUInstallDirs)
 
-install(DIRECTORY include/ct/optcon DESTINATION include/ct)
-
 ## copy the header files
 install(DIRECTORY include/ct/optcon DESTINATION include/ct)
 

From c2fee0eb9e05718d56c68bc811ddbccc54138df1 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Thu, 4 Jun 2020 13:58:04 +0200
Subject: [PATCH 13/21] fast forward with latest fixes.

---
 .../solver/lqp/GNRiccatiSolver-impl.hpp       | 80 +++++++++----------
 1 file changed, 39 insertions(+), 41 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 24ef82b51..52b9686ff 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -9,31 +9,30 @@ 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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
+    : LQOCSolver<MANIFOLD, CONTROL_DIM>(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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::GNRiccatiSolver(int N)
 {
     changeNumberOfStages(N);
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solve()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solve()
 {
     for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
         solveSingleStage(i);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::solveSingleStage(int N)
 {
     if (N == this->lqocProblem_->getNumberOfStages() - 1)
         initializeCostToGo();
@@ -44,16 +43,15 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
         computeCostToGo(N);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::configure(const NLOptConSettings& settings)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::configure(const NLOptConSettings& settings)
 {
     settings_ = settings;
     H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStatesAndControls()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -65,28 +63,33 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        this->x_sol_[k + 1] = p.A_[k] * this->x_sol_[k] + p.B_[k] * (this->u_sol_[k]) + p.b_[k];
+        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backward pass" //TODO: document this
+        StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backward pass"
+        typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backward pass"
+        // Note that we need to transport the state update into the tagent space of k+1
+        this->x_sol_[k + 1] =
+            p.Adj_x_[k + 1].transpose() * (A_orig * this->x_sol_[k] + B_orig * (this->u_sol_[k]) + b_orig);
     }
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeFeedbackMatrices()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeFeedbackMatrices()
 { /*no action required, already computed in backward pass */
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::compute_lv()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::compute_lv()
 { /*no action required, already computed in backward pass*/
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-SCALAR GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSmallestEigenvalue()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+auto GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::getSmallestEigenvalue() -> SCALAR
 {
     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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
 {
     if (lqocProblem->isConstrained())
     {
@@ -98,9 +101,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::setProblemImpl(std::shared
     changeNumberOfStages(N);
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::changeNumberOfStages(int N)
 {
     if (N <= 0)
         return;
@@ -127,9 +129,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N
     N_ = N;
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeCostToGo()
 {
     //! since intializeCostToGo is the first call, we initialize the smallestEigenvalue here.
     smallestEigenvalue_ = std::numeric_limits<SCALAR>::infinity();
@@ -142,9 +143,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
     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)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t k)
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -160,9 +160,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeCostToGo(size_t k)
     sv_[k]/*.noalias()*/ += G_[k].transpose() * this->lv_[k]; // TODO: bring back all the noalias()
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::designController(size_t k)
 {
     LQOCProblem_t& p = *this->lqocProblem_;
 
@@ -259,9 +258,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
     }
 }
 
-
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::logToMatlab()
 {
 #ifdef MATLAB_FULL_LOG
 
@@ -280,8 +278,8 @@ void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
 #endif
 }
 
-template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
-void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeAndAllocate()
+template <typename MANIFOLD, size_t CONTROL_DIM>
+void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::initializeAndAllocate()
 {
     // do nothing
 }

From 4525468fe60f7e64d6d468b05e6ad428ff4d4626 Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Sat, 20 Jun 2020 09:17:40 +0200
Subject: [PATCH 14/21] non-working version

---
 .../core/systems/linearizer/DynamicsLinearizerNumDiff.h   | 8 ++++----
 ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp         | 4 ++++
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
index f9e530370..6ae6d1137 100644
--- a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
+++ b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
@@ -151,8 +151,8 @@ class DynamicsLinearizerNumDiff
             SCALAR dxp = mlog_ph - m_log.coeffs()(i);
 
             t_perturbed = m_log;
-            t_perturbed.set_coeff(i, mlog_ph); // TODO
-            //t_perturbed.coeffs()(i) = mlog_ph;
+            //t_perturbed.set_coeff(i, mlog_ph); // TODO remove
+            t_perturbed.coeffs()(i) = mlog_ph;
 
             // evaluate dynamics at perturbed state
             dynamics_fct_(retract_fct_(t_perturbed), t, u, res_plus);
@@ -163,8 +163,8 @@ class DynamicsLinearizerNumDiff
                 SCALAR dxm = m_log.coeffs()(i) - mlog_mh;
 
                 t_perturbed = m_log;
-                t_perturbed.set_coeff(i, mlog_ph);
-                //t_perturbed.coeffs()(i) = mlog_ph;
+                //t_perturbed.set_coeff(i, mlog_ph); // TODO: remove
+                t_perturbed.coeffs()(i) = mlog_ph;
 
                 Tangent res_minus;
                 dynamics_fct_(retract_fct_(t_perturbed), t, u, res_minus);
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 4ecf41b5a..90d965445 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -60,7 +60,11 @@ class CompositeManifoldTangent
         t_vel_.setZero();
         coeffs_.setZero();
     }
+    
+    // TODO: Here is the problem...
     const Eigen::Matrix<Scalar, DoF, 1>& coeffs() const { return coeffs_; }
+
+
     void set_coeffs(const Eigen::Matrix<Scalar, DoF, 1>& c)
     {
         coeffs_ = c;

From f8ba32152d408783a4d999d48534c7aa8ea2402a Mon Sep 17 00:00:00 2001
From: mgiftthaler <markusgft@gmail.com>
Date: Sun, 21 Jun 2020 17:13:06 +0200
Subject: [PATCH 15/21] update with less coeff() work going on

---
 .../linearizer/DynamicsLinearizerNumDiff.h    | 16 ++++----
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 41 ++++++++-----------
 2 files changed, 24 insertions(+), 33 deletions(-)

diff --git a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
index 6ae6d1137..9137cfb25 100644
--- a/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
+++ b/ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h
@@ -146,25 +146,23 @@ class DynamicsLinearizerNumDiff
         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<SCALAR>(m_log.coeffs()(i)), SCALAR(1.0));
-            SCALAR mlog_ph = m_log.coeffs()(i) + h;
-            SCALAR dxp = mlog_ph - m_log.coeffs()(i);
+            SCALAR h = eps_ * std::max(std::abs<SCALAR>(m_log(i)), SCALAR(1.0));
+            SCALAR mlog_ph = m_log(i) + h;
+            SCALAR dxp = mlog_ph - m_log(i);
 
             t_perturbed = m_log;
-            //t_perturbed.set_coeff(i, mlog_ph); // TODO remove
-            t_perturbed.coeffs()(i) = mlog_ph;
+            t_perturbed(i) = mlog_ph;
 
             // evaluate dynamics at perturbed state
             dynamics_fct_(retract_fct_(t_perturbed), t, u, res_plus);
 
             if (doubleSidedDerivative_)
             {
-                SCALAR mlog_mh = m_log.coeffs()(i) - h;
-                SCALAR dxm = m_log.coeffs()(i) - mlog_mh;
+                SCALAR mlog_mh = m_log(i) - h;
+                SCALAR dxm = m_log(i) - mlog_mh;
 
                 t_perturbed = m_log;
-                //t_perturbed.set_coeff(i, mlog_ph); // TODO: remove
-                t_perturbed.coeffs()(i) = mlog_ph;
+                t_perturbed(i) = mlog_ph;
 
                 Tangent res_minus;
                 dynamics_fct_(retract_fct_(t_perturbed), t, u, res_minus);
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 90d965445..a8f9c1423 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -21,21 +21,20 @@ class CompositeManifoldTangent
     using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
     using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
 
-    CompositeManifoldTangent() : t_pos_(POS_TANGENT::Zero()), t_vel_(VEL_TANGENT::Zero()) { coeffs_.setZero(); }
+    CompositeManifoldTangent() : t_pos_(POS_TANGENT::Zero()), t_vel_(VEL_TANGENT::Zero()) { }
     CompositeManifoldTangent(const POS_TANGENT& p, const VEL_TANGENT& v) : t_pos_(p), t_vel_(v)
     {
-        coeffs_ << t_pos_.coeffs(), t_vel_.coeffs();
     }
 
     template <typename _EigenDerived>
     CompositeManifoldTangent(const Eigen::MatrixBase<_EigenDerived>& v)
-        : t_pos_(v.template head<POS_TANGENT::DoF>()), t_vel_(v.template tail<VEL_TANGENT::DoF>()), coeffs_(v)
+        : t_pos_(v.template head<POS_TANGENT::DoF>()), t_vel_(v.template tail<VEL_TANGENT::DoF>())
     {
         // TODO a dimension assert would not hurt
     }
 
     CompositeManifoldTangent(const CompositeManifoldTangent& arg)
-        : t_pos_(arg.t_pos_), t_vel_(arg.t_vel_), coeffs_(arg.coeffs_)
+        : t_pos_(arg.t_pos_), t_vel_(arg.t_vel_)
     {
     }
 
@@ -52,31 +51,30 @@ class CompositeManifoldTangent
     {
         t_pos_.setRandom();
         t_vel_.setRandom();
-        coeffs_ << t_pos_.coeffs(), t_vel_.coeffs();
     }
     void setZero()
     {
         t_pos_.setZero();
         t_vel_.setZero();
-        coeffs_.setZero();
     }
-    
-    // TODO: Here is the problem...
-    const Eigen::Matrix<Scalar, DoF, 1>& coeffs() const { return coeffs_; }
 
-
-    void set_coeffs(const Eigen::Matrix<Scalar, DoF, 1>& c)
+    // TODO: Here is the problem, trouble as soon as it gets passed by value.
+    const Eigen::Matrix<Scalar, DoF, 1>& coeffs() const
     {
-        coeffs_ = c;
-        t_pos_.coeffs() = c.template head<POS_TANGENT::DoF>();
-        t_vel_.coeffs() = c.template tail<VEL_TANGENT::DoF>();
+        Eigen::Matrix<Scalar, DoF, 1> coeffs_temp;
+        coeffs_temp.template head<3>() = t_pos_.coeffs();
+        coeffs_temp.template tail<3>() = t_vel_.coeffs();
+
+        const_cast<Eigen::Matrix<Scalar, DoF, 1>&>(coeffs_) = coeffs_temp;
+
+        return coeffs_;
     }
 
-    void set_coeff(const int i, const Scalar val)
+
+    Scalar& operator()(const int k) { return k < POS_TANGENT::DoF ? t_pos_.coeffs()(k) : t_vel_.coeffs()(k - 3); }
+    const Scalar& operator()(const int k) const
     {
-        coeffs_(i) = val;
-        t_pos_.coeffs() = coeffs_.template head<POS_TANGENT::DoF>();
-        t_vel_.coeffs() = coeffs_.template tail<VEL_TANGENT::DoF>();
+        return k < POS_TANGENT::DoF ? t_pos_.coeffs()(k) : t_vel_.coeffs()(k - 3);
     }
 
     CompositeManifoldTangent operator-(const CompositeManifoldTangent& tb) const
@@ -118,21 +116,16 @@ class CompositeManifoldTangent
     void set_pos(const POS_TANGENT& p)
     {
         t_pos_ = p;
-        coeffs_.template head<POS_TANGENT::DoF>() = p.coeffs();
     }
     void set_vel(const VEL_TANGENT& v)
     {
         t_vel_ = v;
-        coeffs_.template tail<VEL_TANGENT::DoF>() = v.coeffs();
     }
     const VEL_TANGENT& vel() const { return t_vel_; }
-    //Scalar& operator()(int idx) { return coeffs()(idx); }
-    const Scalar& operator()(int idx) const { return coeffs()(idx); }
-    //auto noalias() { return CompositeManifoldTangent<POS_MAN, VEL_MAN, POS_TANGENT, VEL_TANGENT>(*this); /* coeffs().noalias();*/ }  // TODO: sort this out
 protected:
     POS_TANGENT t_pos_;
     VEL_TANGENT t_vel_;
-    Eigen::Matrix<Scalar, DoF, 1> coeffs_;
+    Eigen::Matrix<Scalar, DoF, 1> coeffs_;  // is now used only in one function to return a ref
 };
 
 template <typename _Stream, typename t1, typename t2, typename t3, typename t4>

From 88cfa2f50021635a978b6c26eca7d66d7dc2685d Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Thu, 25 Jun 2020 21:44:19 +0200
Subject: [PATCH 16/21] non-working hpipm test

---
 .../include/ct/optcon/problem/LQOCProblem.hpp |  2 +
 .../test/solver/linear/ManifoldLQOCTest.cpp   | 75 +++++++++++--------
 2 files changed, 44 insertions(+), 33 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
index ecfa757c3..432aeeea1 100644
--- a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
@@ -89,6 +89,8 @@ class LQOCProblem
 
     ~LQOCProblem();
 
+    LQOCProblem* clone() {return new LQOCProblem(*this);}
+
     //! returns the number of discrete time steps in the LQOCP, including terminal stage
     int getNumberOfStages();
 
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index a8f9c1423..6a26e1e2a 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -21,10 +21,8 @@ class CompositeManifoldTangent
     using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
     using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
 
-    CompositeManifoldTangent() : t_pos_(POS_TANGENT::Zero()), t_vel_(VEL_TANGENT::Zero()) { }
-    CompositeManifoldTangent(const POS_TANGENT& p, const VEL_TANGENT& v) : t_pos_(p), t_vel_(v)
-    {
-    }
+    CompositeManifoldTangent() : t_pos_(POS_TANGENT::Zero()), t_vel_(VEL_TANGENT::Zero()) {}
+    CompositeManifoldTangent(const POS_TANGENT& p, const VEL_TANGENT& v) : t_pos_(p), t_vel_(v) {}
 
     template <typename _EigenDerived>
     CompositeManifoldTangent(const Eigen::MatrixBase<_EigenDerived>& v)
@@ -33,10 +31,7 @@ class CompositeManifoldTangent
         // TODO a dimension assert would not hurt
     }
 
-    CompositeManifoldTangent(const CompositeManifoldTangent& arg)
-        : t_pos_(arg.t_pos_), t_vel_(arg.t_vel_)
-    {
-    }
+    CompositeManifoldTangent(const CompositeManifoldTangent& arg) : t_pos_(arg.t_pos_), t_vel_(arg.t_vel_) {}
 
     static CompositeManifoldTangent Zero()
     {
@@ -113,15 +108,10 @@ class CompositeManifoldTangent
 
     auto transpose() const { return coeffs().transpose(); }
     const POS_TANGENT& pos() const { return t_pos_; }
-    void set_pos(const POS_TANGENT& p)
-    {
-        t_pos_ = p;
-    }
-    void set_vel(const VEL_TANGENT& v)
-    {
-        t_vel_ = v;
-    }
+    void set_pos(const POS_TANGENT& p) { t_pos_ = p; }
+    void set_vel(const VEL_TANGENT& v) { t_vel_ = v; }
     const VEL_TANGENT& vel() const { return t_vel_; }
+
 protected:
     POS_TANGENT t_pos_;
     VEL_TANGENT t_vel_;
@@ -295,6 +285,7 @@ class CompositeManifold
     const POS_MAN& pos() const { return m_pos_; }
     VEL_MAN& vel() { return m_vel_; }
     const VEL_MAN& vel() const { return m_vel_; }
+
 protected:
     POS_MAN m_pos_;
     VEL_MAN m_vel_;
@@ -373,8 +364,11 @@ int main(int argc, char** argv)
         x_traj[i] = ManifoldState_t::Random();
     }
 
-    // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> solver(new GNRiccatiSolver<ManifoldState_t, control_dim>);
+    // create instances of HPIPM and an unconstrained Gauss-Newton Riccati gnSolver
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnSolver(
+        new GNRiccatiSolver<ManifoldState_t, control_dim>);
+    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
+        new HPIPMInterface<ManifoldState_t, control_dim>);
 
     // create linear-quadratic optimal control problem containers
     std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> problem(
@@ -439,31 +433,46 @@ int main(int argc, char** argv)
             auto l_adj = (l.exp()).adj();
             problem->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
 
-            // make the necessary overloads, which allows the standard riccati solver to backpropagate on manifolds
+            // make the necessary overloads, which allows the standard riccati gnSolver to backpropagate on manifolds
             problem->A_[i] = l_adj.transpose() * problem->A_[i];
             problem->B_[i] = l_adj.transpose() * problem->B_[i];
             problem->b_[i] = l_adj.transpose() * problem->b_[i];
         }
 
         // set the problem pointers
-        solver->setProblem(problem);
+        gnSolver->setProblem(problem);
+
+        std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> hpipmProblem (problem->clone());
+        hpipmSolver->setProblem(hpipmProblem);
 
         // allocate memory (if required)
-        solver->initializeAndAllocate();
+        gnSolver->initializeAndAllocate();
+        hpipmSolver->initializeAndAllocate();
 
         // solve the problem
-        solver->solve();
+        gnSolver->solve();
+        hpipmSolver->solve();
 
         // postprocess data
-        solver->compute_lv();
-        solver->computeFeedbackMatrices();
-        solver->computeStatesAndControls();
+        gnSolver->compute_lv();
+        gnSolver->computeFeedbackMatrices();
+        gnSolver->computeStatesAndControls();
+        hpipmSolver->compute_lv();
+        hpipmSolver->computeFeedbackMatrices();
+        hpipmSolver->computeStatesAndControls();
 
         // retrieve solutions from solver
-        auto xSol = solver->getSolutionState();
-        auto uSol = solver->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol = solver->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol = solver->get_lv();
+        auto xSol_gn = gnSolver->getSolutionState();
+        auto uSol_gn = gnSolver->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_gn = gnSolver->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_gn = gnSolver->get_lv();
+
+        auto xSol_hpipm = hpipmSolver->getSolutionState();
+        auto uSol_hpipm = hpipmSolver->getSolutionControl();
+        ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = hpipmSolver->getSolutionFeedback();
+        ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = hpipmSolver->get_lv();
+
+        // TODO: include a comparison here.
 
         x_curr = x0;
         ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
@@ -477,14 +486,14 @@ int main(int argc, char** argv)
             if (use_single_shooting)
             {
                 ManifoldState_t::Tangent x_err = x_traj[i].rminus(x_traj_prev[i]);
-                u_traj[i] += lv_sol[i] + KSol[i] * x_err;
+                u_traj[i] += lv_sol_gn[i] + KSol_gn[i] * x_err;
                 exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
                 x_traj[i + 1] = x_traj[i] + dx;
             }
             else  // multiple shooting
             {
-                u_traj[i] += uSol[i];
-                x_traj[i + 1] = x_traj[i + 1].rplus(xSol[i + 1]);
+                u_traj[i] += uSol_gn[i];
+                x_traj[i + 1] = x_traj[i + 1].rplus(xSol_gn[i + 1]);
                 exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
             }
 
@@ -493,7 +502,7 @@ int main(int argc, char** argv)
 
             // compute update norms
             d_cum_sum += b[i].coeffs().norm();
-            dx_cum_sum += xSol[i + 1].coeffs().norm();
+            dx_cum_sum += xSol_gn[i + 1].coeffs().norm();
 
             // compute intermediate cost
             costFunction->setCurrentStateAndControl(x_traj[i], u_traj[i], i * dt);

From d6572d37b1d2471cf059e35df82143f77abe5d6e Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Mon, 13 Jul 2020 08:18:51 +0200
Subject: [PATCH 17/21] Add impl for eigen-type to csv export.

---
 .../include/ct/core/common/EigenFileExport.h  | 132 ++++++++++++++++++
 1 file changed, 132 insertions(+)
 create mode 100644 ct_core/include/ct/core/common/EigenFileExport.h

diff --git a/ct_core/include/ct/core/common/EigenFileExport.h b/ct_core/include/ct/core/common/EigenFileExport.h
new file mode 100644
index 000000000..7d0c9aab2
--- /dev/null
+++ b/ct_core/include/ct/core/common/EigenFileExport.h
@@ -0,0 +1,132 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
+Licensed under the BSD-2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <string>
+#include <fstream>
+#include <vector>
+
+#include <Eigen/Dense>
+
+namespace ct::core {
+
+//
+// @brief Save Eigen-types or vectors of Eigen-types to text file.
+// Currently ordinary Eigen matrices and quaternion types are supported.
+//
+// Usage examples:
+// // writing an eigen matrix
+// Eigen::Matrix3d m = Eigen::Matrix3d::Random();
+// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat.csv", m, "test mat");
+//
+// // writing a vector of eigen matrices
+// std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> m_vec(3, Eigen::Matrix3d::Random());
+// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat_vec.csv", m_vec, "some title.");
+//
+// // writing a quaternion
+// Eigen::Quaterniond q(Eigen::Quaterniond::Identity());
+// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat.csv", q, "test quat");
+//
+// // writing a vector of quaternions
+// std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> q_vec(
+//     5, Eigen::Quaterniond::Identity());
+// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat_vec.csv", q_vec);
+//
+class EigenFileExport
+{
+public:
+    // Create CSV formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CSVFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", "\n");
+    }
+    // Create Octave formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat OctaveFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", ";\n", "", "", "[", "]");
+    }
+    // Create comma init formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CommaInitFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";");
+    }
+    // Create a clean formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat CleanFormat(int precision = Eigen::StreamPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", "\n", "[", "]");
+    }
+    // create default full-info formatting option, precision can be custimized if desired.
+    static const Eigen::IOFormat HeavyFormat(int precision = Eigen::FullPrecision)
+    {
+        return Eigen::IOFormat(precision, 0, ", ", ";\n", "[", "]", "[", "]");
+    }
+
+    //! Save a single matrix to single text file.
+    template <typename M>
+    static void mat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const Eigen::MatrixBase<M>& m,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+        if (!heading.empty())
+            file << heading << "\n";
+        file << m.format(fmt);
+
+        file.close();
+    }
+
+    //! Save a single quaternion to a single text file.
+    template <typename M>
+    static void quat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const Eigen::QuaternionBase<M>& q,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+        if (!heading.empty())
+            file << heading << "\n";
+        file << q.coeffs().transpose().format(fmt);
+
+        file.close();
+    }
+
+    // Save a vector of matrices to a single text file.
+    template <typename M, typename Alloc = Eigen::aligned_allocator<M>>
+    static void mat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const std::vector<M, Alloc>& m_vec,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+
+        if (!heading.empty())
+            file << heading << "\n";
+
+        for (size_t i = 0; i < m_vec.size(); i++)
+            file << m_vec[i].format(fmt) << "\n";
+
+        file.close();
+    }
+
+    // Save a vector of quaternions to a single text file.
+    template <typename Q, typename Alloc = Eigen::aligned_allocator<Eigen::Quaternion<Q>>>
+    static void quat_to_file(const Eigen::IOFormat fmt,
+        const std::string& filename,
+        const std::vector<Eigen::Quaternion<Q>, Alloc>& q_vec,
+        const std::string& heading = "")
+    {
+        std::ofstream file(filename.c_str());
+
+        if (!heading.empty())
+            file << heading << "\n";
+
+        for (size_t i = 0; i < q_vec.size(); i++)
+            file << q_vec[i].coeffs().transpose().format(fmt) << "\n";
+
+        file.close();
+    }
+};
+
+}  // namespace ct::core

From b1940a93022095f142924aaa1b91afbd05607923 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Mon, 13 Jul 2020 16:24:02 +0200
Subject: [PATCH 18/21] backup

---
 ct_core/CMakeLists.txt                        |   2 +-
 ct_core/include/ct/core/Common                |   1 +
 .../include/ct/core/types/EuclideanState.h    |  19 +++
 .../CostFunctionQuadraticSimple-impl.hpp      |   2 +-
 ct_optcon/test/CMakeLists.txt                 |  10 +-
 .../test/solver/linear/HPIPMInterfaceTest.h   |  10 +-
 .../linear/KinematicManifoldLQOCTest.cpp      | 113 +++++++-----------
 ct_optcon/test/solver/linear/LQOCSolverTest.h |   4 +-
 .../test/solver/linear/ManifoldLQOCTest.cpp   |  27 +++--
 9 files changed, 91 insertions(+), 97 deletions(-)

diff --git a/ct_core/CMakeLists.txt b/ct_core/CMakeLists.txt
index 993685ee4..139453ca3 100755
--- a/ct_core/CMakeLists.txt
+++ b/ct_core/CMakeLists.txt
@@ -157,7 +157,7 @@ target_link_libraries(ct_core INTERFACE
     dl # required for gcc compatibility
     )
 
-
+    
 ##################
 # BUILD EXAMPLES #
 ##################
diff --git a/ct_core/include/ct/core/Common b/ct_core/include/ct/core/Common
index 1d34208d7..017bc09b2 100644
--- a/ct_core/include/ct/core/Common
+++ b/ct_core/include/ct/core/Common
@@ -15,5 +15,6 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 #include "common/linspace.h"
 #include "common/activations/Activations.h"
 #include "common/EigenIsNan.h"
+#include "common/EigenFileExport.h"
 
 
diff --git a/ct_core/include/ct/core/types/EuclideanState.h b/ct_core/include/ct/core/types/EuclideanState.h
index 57b1ccd83..25eb4ceb6 100644
--- a/ct_core/include/ct/core/types/EuclideanState.h
+++ b/ct_core/include/ct/core/types/EuclideanState.h
@@ -6,6 +6,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory)
 #pragma once
 
 #include <Eigen/Dense>
+#include "lt/optional.hpp"
+
 
 namespace ct {
 namespace core {
@@ -53,6 +55,23 @@ class EuclideanState : public Eigen::Matrix<SCALAR, DIM, 1>
 
     Tangent rminus(const EuclideanState& x) const { return *this - x; }
     EuclideanState rplus(const Tangent& x) const { return *this + x; }
+
+    using OptJacobianRef = tl::optional<Eigen::Ref<Eigen::Matrix<SCALAR, DIM, DIM>>>;
+    Tangent rminus(const EuclideanState& m, OptJacobianRef J_t_ma, OptJacobianRef J_t_mb) const
+    {
+        const Tangent t = rminus(m);
+
+        if (J_t_ma)
+        {
+            (*J_t_ma).setIdentity();
+        }
+        if (J_t_mb)
+        {
+            (*J_t_mb).setIdentity();
+        }
+
+        return t;
+    }
 };
 
 template <size_t DIM, class SCALAR = double>
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index 0eb40671a..aa7282bf6 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -137,7 +137,7 @@ template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    x_final_.rminus(this->x_, Jl, Jr);
     return Jr * Q_final_ * Jr.transpose();
 }
 
diff --git a/ct_optcon/test/CMakeLists.txt b/ct_optcon/test/CMakeLists.txt
index 1e54468b1..0067cb119 100644
--- a/ct_optcon/test/CMakeLists.txt
+++ b/ct_optcon/test/CMakeLists.txt
@@ -53,10 +53,10 @@ if(BUILD_TESTS)
     #package_add_test(dms_test_all_var dms/oscillator/oscDMSTestAllVariants.cpp)
     #package_add_test(system_interface_test system_interface/SystemInterfaceTest.cpp)
     
-    #add_executable(KinematicManifoldLQOCTest solver/linear/KinematicManifoldLQOCTest.cpp)
-    #target_link_libraries(KinematicManifoldLQOCTest ct_optcon)
-    #set_target_properties(KinematicManifoldLQOCTest PROPERTIES FOLDER test)
-    #list(APPEND UNIT_TEST_TARGETS KinematicManifoldLQOCTest)    
+    add_executable(KinematicManifoldLQOCTest solver/linear/KinematicManifoldLQOCTest.cpp)
+    target_link_libraries(KinematicManifoldLQOCTest ct_optcon)
+    set_target_properties(KinematicManifoldLQOCTest PROPERTIES FOLDER test)
+    list(APPEND UNIT_TEST_TARGETS KinematicManifoldLQOCTest)    
     
     add_executable(ManifoldLQOCTest solver/linear/ManifoldLQOCTest.cpp)
     target_link_libraries(ManifoldLQOCTest ct_optcon ct_core)
@@ -69,7 +69,7 @@ if(BUILD_TESTS)
      #   add_executable(LQOCSolverTiming solver/linear/LQOCSolverTiming.cpp)
      #   target_link_libraries(LQOCSolverTiming ct_optcon)
         
-     #   package_add_test(LQOCSolverTest solver/linear/LQOCSolverTest.cpp)
+      #package_add_test(LQOCSolverTest solver/linear/LQOCSolverTest.cpp)
      #   package_add_test(ConstrainedLQOCSolverTest solver/linear/ConstrainedLQOCSolverTest.cpp)
      #   package_add_test(LinearSystemSolverComparison nloc/LinearSystemSolverComparison.cpp)
       #  if(CPPADCG)
diff --git a/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
index a98b4d5e3..e6e6d64f6 100644
--- a/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
+++ b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
@@ -55,8 +55,10 @@ TEST(HPIPMInterfaceTest, compareSolvers)
         dt, system, ct::optcon::NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
 
     // initialize the linear quadratic optimal control problems
-    lqocProblem_hpipm->setFromTimeInvariantLinearQuadraticProblem(discretizedSystem, costFunction, stateOffset, dt);
-    lqocProblem_gnriccati->setFromTimeInvariantLinearQuadraticProblem(discretizedSystem, costFunction, stateOffset, dt);
+    lqocProblem_hpipm->setFromTimeInvariantLinearQuadraticProblem(
+        x0, u0, discretizedSystem, costFunction, stateOffset, dt);
+    lqocProblem_gnriccati->setFromTimeInvariantLinearQuadraticProblem(
+        x0, u0, discretizedSystem, costFunction, stateOffset, dt);
 
 
     // create hpipm solver instance, set and solve problem
@@ -76,8 +78,8 @@ TEST(HPIPMInterfaceTest, compareSolvers)
     gnriccati.compute_lv();
 
     // compute and retrieve solutions
-    ct::core::DiscreteArray<State> x_sol_hpipm = hpipm.getSolutionState();
-    ct::core::DiscreteArray<State> x_sol_gnrccati = gnriccati.getSolutionState();
+    ct::core::DiscreteArray<typename State::Tangent> x_sol_hpipm = hpipm.getSolutionState();
+    ct::core::DiscreteArray<typename State::Tangent> x_sol_gnrccati = gnriccati.getSolutionState();
     ct::core::ControlVectorArray<control_dim> u_sol_hpipm = hpipm.getSolutionControl();
     ct::core::ControlVectorArray<control_dim> u_sol_gnrccati = gnriccati.getSolutionControl();
     ct::core::FeedbackArray<state_dim, control_dim> K_sol_hpipm = hpipm.getSolutionFeedback();
diff --git a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
index 227bfaaef..5b4e1279e 100644
--- a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
@@ -1,14 +1,8 @@
 
 #include <ct/optcon/optcon.h>
-#include <ct/optcon/solver/lqp/AugGNRiccatiSolver.hpp>
-#include <ct/optcon/solver/lqp/AugGNRiccatiSolver-impl.hpp>
-
-//#include "matplotlibcpp.h"
 
 using namespace ct::core;
 using namespace ct::optcon;
-
-
 const bool verbose = true;
 
 using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
@@ -90,16 +84,16 @@ int main(int argc, char** argv)
 
     const bool use_single_shooting = true;  // toggle between single and multiple shooting
 
-    const size_t N = 250;
-    const double dt = 0.5;
+    const size_t N = 17;
+    const double dt = 0.1;
 
-    const ManifoldState_t x0 = manif::SO3<double>(3.1, 0, 0);
+    const ManifoldState_t x0 = manif::SO3<double>(M_PI / 2, 0, 0);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
     ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
         N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
     ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
     for (size_t i = 0; i < N; i++)
-        u_traj[i] = ct::core::ControlVector<control_dim>::Random()* 0.01;
+        u_traj[i] = ct::core::ControlVector<control_dim>::Random() * 0.01;
 
     // choose a random initial state
     // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
@@ -111,14 +105,14 @@ int main(int argc, char** argv)
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
     std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnRiccatiSolver(
         new GNRiccatiSolver<ManifoldState_t, control_dim>);
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> augGnRiccatiSolver(
-        new AugGNRiccatiSolver<ManifoldState_t, control_dim>);
+    // std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
+    //     new HPIPMInterface<ManifoldState_t, control_dim>);
 
     // store them, and identifying names, in a vectors
     std::vector<std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>>> lqocSolvers;
     lqocSolvers.push_back(gnRiccatiSolver);
-    lqocSolvers.push_back(augGnRiccatiSolver);
-    std::vector<std::string> solverNames = {"Riccati", "AugRiccati"};
+    // lqocSolvers.push_back(hpipmSolver);
+    std::vector<std::string> solverNames = {"Riccati", "hpipm"};
 
     // create linear-quadratic optimal control problem containers
     std::vector<std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>>> problems;
@@ -140,13 +134,15 @@ int main(int argc, char** argv)
     // create a cost function
     Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
     Eigen::Matrix<double, control_dim, control_dim> R;
-    Q_final << 10000, 0, 0, 0, 10000, 0, 0, 0, 10000;
-    Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;
-    //Q.setZero();
-    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    Q_final.setZero();
+    Q_final.diagonal() << 10000, 10000, 10000;
+    Q.setZero();
+    Q.diagonal() << 1, 1, 1;
+    R.setZero();
+    R.diagonal() << 1, 1, 1;
     ManifoldState_t x_final = manif::SO3<double>(0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
-    ManifoldState_t x_nominal = x0;
+    ManifoldState_t x_nominal = x_final;
     ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
     std::shared_ptr<CostFunctionQuadratic<ManifoldState_t, control_dim>> costFunction(
         new CostFunctionQuadraticSimple<ManifoldState_t, control_dim>(Q, R, x_nominal, u_nom, x_final, Q_final));
@@ -170,7 +166,7 @@ int main(int argc, char** argv)
         std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
-    size_t nIter = 35;
+    size_t nIter = 25;
     for (size_t iter = 0; iter < nIter; iter++)
     {
         // initialize the optimal control problems for both solvers
@@ -179,35 +175,14 @@ int main(int argc, char** argv)
 
 
         // HACKY corrections // TODO: move somewhere meaningful
-        for (size_t idx : {0, 1})
+        for (size_t idx : {0})
         {
-            // intermediate stages cost transportation
-            for (size_t i = 0; i < N; i++)
-            {
-                auto e = x_nominal.rminus(x_traj[i]);
-                // compute PT matrix w.r.t. current ref traj // TODO: clarify formulation of error in cost function
-                auto e_adj = (e.exp()).adj();
-                problems[idx]->Q_[i] = e_adj * problems[idx]->Q_[i] *
-                                       e_adj.transpose();  // TODO: sort out that thing with the cost function
-                problems[idx]->qv_[i] = e_adj * problems[idx]->qv_[i];
-            }
-            // terminal stage transportation
-            // TODO: clarifiy formulation of error in cost function
-            auto e = x_final.rminus(x_traj[N]);  // compute PT matrix w.r.t. current ref traj
-            auto e_adj = (e.exp()).adj();
-            //std::cout << "cost adj" << std::endl << e_adj << std::endl;
-            problems[idx]->Q_.back() = e_adj * problems[idx]->Q_.back() * e_adj.transpose();
-            problems[idx]->qv_.back() = e_adj * problems[idx]->qv_.back();
-
             // dynamics transportation
             for (size_t i = 0; i < N; i++)
             {
-                // std::cout << "dyn transport matrices" << std::endl;
-                Eigen::Matrix3d Jl, Jr;
-                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);
+                auto l = x_traj[i + 1].rminus(x_traj[i]);
                 auto l_adj = (l.exp()).adj();
-                //std::cout << "l_adj" << std::endl << l_adj << std::endl;
-                //std::cout << std::endl;
+
                 problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
 
                 if (idx == 0)  // make the corrections for the standard riccati solver
@@ -216,16 +191,6 @@ int main(int argc, char** argv)
                     problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
                     problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
                 }
-
-                // now just a test // todo: remove later
-                //auto temp_l = x_traj[i + 1].between(x_traj[i]);
-                //auto temp_l_adj = temp_l.adj();
-                //if (!(l_adj.isApprox(temp_l_adj.transpose(), 1e-5)))
-                //{
-                //    std::cout << std::setprecision(10) << "l_adj: " << std::endl << l_adj << std::endl;
-                //    std::cout << std::setprecision(10) << "temp_l_adj: " << std::endl << temp_l_adj << std::endl;
-                //    throw std::runtime_error("No- that does not work.");
-                //}
             }
 
             // set the problem pointers
@@ -249,43 +214,43 @@ int main(int argc, char** argv)
         ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati = lqocSolvers[0]->getSolutionFeedback();
         ct::core::ControlVectorArray<control_dim> lv_sol_riccati = lqocSolvers[0]->get_lv();
 
-        auto xSol_aug_riccati = lqocSolvers[1]->getSolutionState();
-        auto uSol_aug_riccati = lqocSolvers[1]->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol_aug_riccati = lqocSolvers[1]->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol_aug_riccati = lqocSolvers[1]->get_lv();
+        // auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
+        // auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
+        // ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = lqocSolvers[1]->getSolutionFeedback();
+        // ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = lqocSolvers[1]->get_lv();
 
         // compare the quantities
         //for (size_t i = 0; i < lv_sol_riccati.size(); i++)
         //{
-        //    if ((lv_sol_riccati[i] - lv_sol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    if ((lv_sol_riccati[i] - lv_sol_hpipm[i]).array().abs().maxCoeff() > 1e-8)
         //    {
         //        std::cout << std::setprecision(10) << lv_sol_riccati[i].transpose() << std::endl;
-        //        std::cout << std::setprecision(10) << lv_sol_aug_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << lv_sol_hpipm[i].transpose() << std::endl;
         //        throw std::runtime_error("lv solutions do not match");
         //    }
         //}
         //for (size_t i = 0; i < KSol_riccati.size(); i++)
         //{
-        //    if (KSol_riccati[i].isApprox(KSol_aug_riccati[i], 1e-6) == false)
+        //    if (KSol_riccati[i].isApprox(KSol_hpipm[i], 1e-6) == false)
         //        throw std::runtime_error("K solutions do not match");
         //}
-//
+        //
         //for (size_t i = 0; i < uSol_riccati.size(); i++)
         //{
-        //    if ((uSol_riccati[i] - uSol_aug_riccati[i]).array().abs().maxCoeff() > 1e-8)
+        //    if ((uSol_riccati[i] - uSol_hpipm[i]).array().abs().maxCoeff() > 1e-8)
         //    {
         //        std::cout << "for index " << i << std::endl;
         //        std::cout << std::setprecision(10) << uSol_riccati[i].transpose() << std::endl;
-        //        std::cout << std::setprecision(10) << uSol_aug_riccati[i].transpose() << std::endl;
+        //        std::cout << std::setprecision(10) << uSol_hpipm[i].transpose() << std::endl;
         //        throw std::runtime_error("u solutions do not match");
         //    }
         //}
         //for (size_t i = 0; i < xSol_riccati.size(); i++)
         //{
-        //    if ((xSol_riccati[i] - xSol_aug_riccati[i]).coeffs().array().abs().maxCoeff() > 1e-8)
+        //    if ((xSol_riccati[i] - xSol_hpipm[i]).coeffs().array().abs().maxCoeff() > 1e-8)
         //    {
         //        std::cout << xSol_riccati[i].transpose() << std::endl;
-        //        std::cout << xSol_aug_riccati[i].transpose() << std::endl;
+        //        std::cout << xSol_hpipm[i].transpose() << std::endl;
         //        throw std::runtime_error("x solutions do not match");
         //    }
         //}
@@ -309,9 +274,9 @@ int main(int argc, char** argv)
         //              << "\t -- rot diff norm(): " << angularDiff << std::endl;
         //}
 
-        std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
+        //std::cout << std::setprecision(4) << "Forward integrated closed-loop solution, iter :" << iter << std::endl;
         x_curr = x0;
-        std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        //std::cout << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
         ct::core::DiscreteArray<ManifoldState_t> x_traj_prev = x_traj;
         double d_cum_sum = 0;
         double dx_cum_sum = 0;
@@ -356,9 +321,15 @@ int main(int argc, char** argv)
         costFunction->setCurrentStateAndControl(x_traj.back(), u_traj.back(), N * dt);
         cost_sum += costFunction->evaluateTerminal();
 
-        std::cout << "d_norm: \t " << d_cum_sum << "\t dx_norm: \t" << dx_cum_sum << "\t cost: " << cost_sum
-                  << std::endl;
-        std::cout << std::endl << std::endl;
+        std::cout << std::setprecision(10) << "d_norm: \t " << d_cum_sum << "\t dx_norm: \t" << dx_cum_sum
+                  << " \t Jcost: " << cost_sum << std::endl;
     }  // end iter
+
+
+    // save the x-trajectory to file
+    std::vector<Eigen::Matrix3d> rot_traj;
+    for(size_t i = 0; i<x_traj.size(); i++)
+        rot_traj.push_back(x_traj[i].rotation());
+    EigenFileExport::mat_to_file(EigenFileExport::CSVFormat(), "/tmp/rot_traj.csv", rot_traj);
     return 1;
 }
\ No newline at end of file
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTest.h b/ct_optcon/test/solver/linear/LQOCSolverTest.h
index d97f89231..1e4c6bac3 100644
--- a/ct_optcon/test/solver/linear/LQOCSolverTest.h
+++ b/ct_optcon/test/solver/linear/LQOCSolverTest.h
@@ -60,8 +60,8 @@ TEST(LQOCSolverTest, compareHPIPMandRiccati)
     b << 0.1, 0.1;
 
     // initialize the optimal control problems for both solvers
-    problems[0]->setFromTimeInvariantLinearQuadraticProblem(discreteExampleSystem, *costFunction, b, dt);
-    problems[1]->setFromTimeInvariantLinearQuadraticProblem(discreteExampleSystem, *costFunction, b, dt);
+    problems[0]->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, b, dt);
+    problems[1]->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, b, dt);
 
     // set the problem pointers
     lqocSolvers[0]->setProblem(problems[0]);
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 6a26e1e2a..618146b35 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -106,6 +106,7 @@ class CompositeManifoldTangent
         return c;
     }
 
+    const Scalar* data() const { return coeffs().data(); }
     auto transpose() const { return coeffs().transpose(); }
     const POS_TANGENT& pos() const { return t_pos_; }
     void set_pos(const POS_TANGENT& p) { t_pos_ = p; }
@@ -367,8 +368,8 @@ int main(int argc, char** argv)
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati gnSolver
     std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> gnSolver(
         new GNRiccatiSolver<ManifoldState_t, control_dim>);
-    std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
-        new HPIPMInterface<ManifoldState_t, control_dim>);
+    // std::shared_ptr<LQOCSolver<ManifoldState_t, control_dim>> hpipmSolver(
+    //     new HPIPMInterface<ManifoldState_t, control_dim>);
 
     // create linear-quadratic optimal control problem containers
     std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> problem(
@@ -442,24 +443,24 @@ int main(int argc, char** argv)
         // set the problem pointers
         gnSolver->setProblem(problem);
 
-        std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> hpipmProblem (problem->clone());
-        hpipmSolver->setProblem(hpipmProblem);
+        // std::shared_ptr<LQOCProblem<ManifoldState_t, control_dim>> hpipmProblem (problem->clone());
+        // hpipmSolver->setProblem(hpipmProblem);
 
         // allocate memory (if required)
         gnSolver->initializeAndAllocate();
-        hpipmSolver->initializeAndAllocate();
+        // hpipmSolver->initializeAndAllocate();
 
         // solve the problem
         gnSolver->solve();
-        hpipmSolver->solve();
+        // hpipmSolver->solve();
 
         // postprocess data
         gnSolver->compute_lv();
         gnSolver->computeFeedbackMatrices();
         gnSolver->computeStatesAndControls();
-        hpipmSolver->compute_lv();
-        hpipmSolver->computeFeedbackMatrices();
-        hpipmSolver->computeStatesAndControls();
+        // hpipmSolver->compute_lv();
+        // hpipmSolver->computeFeedbackMatrices();
+        // hpipmSolver->computeStatesAndControls();
 
         // retrieve solutions from solver
         auto xSol_gn = gnSolver->getSolutionState();
@@ -467,10 +468,10 @@ int main(int argc, char** argv)
         ct::core::FeedbackArray<state_dim, control_dim> KSol_gn = gnSolver->getSolutionFeedback();
         ct::core::ControlVectorArray<control_dim> lv_sol_gn = gnSolver->get_lv();
 
-        auto xSol_hpipm = hpipmSolver->getSolutionState();
-        auto uSol_hpipm = hpipmSolver->getSolutionControl();
-        ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = hpipmSolver->getSolutionFeedback();
-        ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = hpipmSolver->get_lv();
+        // auto xSol_hpipm = hpipmSolver->getSolutionState();
+        // auto uSol_hpipm = hpipmSolver->getSolutionControl();
+        // ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm = hpipmSolver->getSolutionFeedback();
+        // ct::core::ControlVectorArray<control_dim> lv_sol_hpipm = hpipmSolver->get_lv();
 
         // TODO: include a comparison here.
 

From 39682a7cf581b983914e7128cef657d033119901 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Mon, 13 Jul 2020 19:25:56 +0200
Subject: [PATCH 19/21] reverse backup

---
 .../CostFunctionQuadraticSimple-impl.hpp      | 10 +++-
 .../linear/KinematicManifoldLQOCTest.cpp      | 56 +++++++++++--------
 2 files changed, 41 insertions(+), 25 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index aa7282bf6..f40824a1a 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -122,7 +122,8 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> S
     // TODO: including Jr_ might not be required here (identical result)
     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
     typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
-    return SCALAR(0.5) * (x_deviation_final.transpose() * Jr * Q_final_ * Jr.transpose() * x_deviation_final)(0);
+    // return SCALAR(0.5) * (x_deviation_final.transpose() * Jr * Q_final_ * Jr.transpose() * x_deviation_final)(0);
+    return SCALAR(0.5) * (x_deviation_final.transpose() * Q_final_ * x_deviation_final)(0);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -130,7 +131,8 @@ typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::s
 {
     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
     typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
-    return Q_final_ * Jr.transpose() * x_deviation_final;
+    return Q_final_ * x_deviation_final;
+    // return Q_final_ * Jr.transpose() * x_deviation_final;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -138,7 +140,9 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTe
 {
     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
     x_final_.rminus(this->x_, Jl, Jr);
-    return Jr * Q_final_ * Jr.transpose();
+
+    return Q_final_;
+    // return Jr * Q_final_ * Jr.transpose();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
index 5b4e1279e..27e1ca2d2 100644
--- a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
@@ -5,16 +5,16 @@ using namespace ct::core;
 using namespace ct::optcon;
 const bool verbose = true;
 
-using ManifoldState_t = ManifoldState<manif::SO3, manif::SO3Tangent>;
+using ManifoldState_t = ManifoldState<manif::SE3, manif::SE3Tangent>;
 const size_t state_dim = ManifoldState_t::TangentDim;
-const size_t control_dim = 3;
+const size_t control_dim = 6;
 
-class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>
+class DiscrSE3LTITestSystem final : public ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-    DiscrSO3LTITestSystem() {}
+    DiscrSE3LTITestSystem() {}
     virtual void computeControlledDynamics(const ManifoldState_t& m,
         const Time_t& n,
         const ct::core::ControlVector<control_dim>& u,
@@ -23,7 +23,7 @@ class DiscrSO3LTITestSystem final : public ct::core::ControlledSystem<ManifoldSt
         dx = u;
     }
 
-    virtual DiscrSO3LTITestSystem* clone() const override { return new DiscrSO3LTITestSystem(); }
+    virtual DiscrSE3LTITestSystem* clone() const override { return new DiscrSE3LTITestSystem(); }
     /**
      * @brief the log operator is defined as expressing the tangent vector w.r.t. m_ref_
      */
@@ -84,16 +84,16 @@ int main(int argc, char** argv)
 
     const bool use_single_shooting = true;  // toggle between single and multiple shooting
 
-    const size_t N = 17;
-    const double dt = 0.1;
+    const size_t N = 150;
+    const double dt = 0.01;
 
-    const ManifoldState_t x0 = manif::SO3<double>(M_PI / 2, 0, 0);
+    const ManifoldState_t x0 = manif::SE3<double>(0, 0, 0, 3.14, 0, 0);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
     ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
         N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
     ct::core::DiscreteArray<ct::core::ControlVector<control_dim>> u_traj(N);  // init control traj
     for (size_t i = 0; i < N; i++)
-        u_traj[i] = ct::core::ControlVector<control_dim>::Random() * 0.01;
+        u_traj[i] = ct::core::ControlVector<control_dim>::Random() * 0.0;
 
     // choose a random initial state
     // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
@@ -126,7 +126,7 @@ int main(int argc, char** argv)
 
     // create a discrete-time manifold system
     std::shared_ptr<ct::core::ControlledSystem<ManifoldState_t, control_dim, DISCRETE_TIME>> exampleSystem(
-        new DiscrSO3LTITestSystem());
+        new DiscrSE3LTITestSystem());
     std::shared_ptr<ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>> linearizer(
         new ct::core::SystemLinearizer<ManifoldState_t, control_dim, DISCRETE_TIME>(exampleSystem));
 
@@ -135,12 +135,12 @@ int main(int argc, char** argv)
     Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
     Eigen::Matrix<double, control_dim, control_dim> R;
     Q_final.setZero();
-    Q_final.diagonal() << 10000, 10000, 10000;
+    Q_final.diagonal() << 10000, 10000, 10000, 10000, 10000, 10000;
     Q.setZero();
-    Q.diagonal() << 1, 1, 1;
+    //Q.diagonal() << 1, 1, 1, 1, 1, 1;
     R.setZero();
-    R.diagonal() << 1, 1, 1;
-    ManifoldState_t x_final = manif::SO3<double>(0, 0, 0);
+    R.diagonal() << 300, 300, 300, 300, 300, 300;
+    ManifoldState_t x_final = manif::SE3<double>(1, 1, 1, 0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
     ManifoldState_t x_nominal = x_final;
     ct::core::ControlVector<control_dim> u_nom = ct::core::ControlVector<control_dim>::Zero();
@@ -153,8 +153,8 @@ int main(int argc, char** argv)
     ManifoldState_t::Tangent dx;
     x_curr = x0;
     x_traj.front() = x0;
-    std::cout << "integrate an random initial state with the unstable system" << std::endl;
-    std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+    // std::cout << "integrate an random initial state with the unstable system" << std::endl;
+    // std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     for (size_t i = 0; i < N; i++)
     {
         exampleSystem->computeControlledDynamics(x_traj[i], 0, u_traj[i], dx);
@@ -162,11 +162,11 @@ int main(int argc, char** argv)
         if (use_single_shooting)
             x_traj[i + 1] = x_curr;
         b[i] = dx - x_traj[i + 1].rminus(x_traj[i]);
-        std::cout << "b: " << b[i] << std::endl;
-        std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
+        // std::cout << "b: " << b[i] << std::endl;
+        // std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
-    size_t nIter = 25;
+    size_t nIter = 5;
     for (size_t iter = 0; iter < nIter; iter++)
     {
         // initialize the optimal control problems for both solvers
@@ -193,6 +193,11 @@ int main(int argc, char** argv)
                 }
             }
 
+            auto l = x_final.rminus(x_traj.back());
+            auto l_adj = (l.exp()).adj();
+            problems[idx]->Q_.back() = l_adj.transpose() * problems[idx]->Q_.back() * l_adj;
+            problems[idx]->qv_.back().coeffs() = - l_adj.transpose() * problems[idx]->qv_.back().coeffs();
+
             // set the problem pointers
             lqocSolvers[idx]->setProblem(problems[idx]);
 
@@ -284,7 +289,8 @@ int main(int argc, char** argv)
         for (size_t i = 0; i < N; i++)
         {
             dx.setZero();
-            Eigen::Quaterniond old_rot(x_traj[i].w(), x_traj[i].x(), x_traj[i].y(), x_traj[i].z());
+            Eigen::Quaterniond old_rot(
+                x_traj[i].quat().w(), x_traj[i].quat().x(), x_traj[i].quat().y(), x_traj[i].quat().z());
 
             if (use_single_shooting)
             {
@@ -301,7 +307,8 @@ int main(int argc, char** argv)
                 exampleSystem->computeControlledDynamics(x_traj[i], i * dt, u_traj[i], dx);
             }
 
-            Eigen::Quaterniond new_rot(x_traj[i + 1].w(), x_traj[i + 1].x(), x_traj[i + 1].y(), x_traj[i + 1].z());
+            Eigen::Quaterniond new_rot(
+                x_traj[i + 1].quat().w(), x_traj[i + 1].quat().x(), x_traj[i + 1].quat().y(), x_traj[i + 1].quat().z());
             //std::cout << "m: " << x_traj[i + 1] << "\t dx: " << xSol_riccati[i + 1]
             //          << "\t -- rot diff norm(): " << old_rot.angularDistance(new_rot) << std::endl;
 
@@ -328,8 +335,13 @@ int main(int argc, char** argv)
 
     // save the x-trajectory to file
     std::vector<Eigen::Matrix3d> rot_traj;
-    for(size_t i = 0; i<x_traj.size(); i++)
+    std::vector<Eigen::Vector3d> trans_traj;
+    for (size_t i = 0; i < x_traj.size(); i++)
+    {
         rot_traj.push_back(x_traj[i].rotation());
+        trans_traj.push_back(x_traj[i].translation());
+    }
     EigenFileExport::mat_to_file(EigenFileExport::CSVFormat(), "/tmp/rot_traj.csv", rot_traj);
+    EigenFileExport::mat_to_file(EigenFileExport::CSVFormat(), "/tmp/trans_traj.csv", trans_traj);
     return 1;
 }
\ No newline at end of file

From 94fb5a43b62f7410dd37544c18d8fd5c5ee85765 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Tue, 14 Jul 2020 22:56:18 +0200
Subject: [PATCH 20/21] kinematic case not working with intermediate cost

---
 .../CostFunctionQuadraticSimple-impl.hpp      | 39 +++++++++----------
 .../CostFunctionQuadraticSimple.hpp           |  3 +-
 .../solver/lqp/GNRiccatiSolver-impl.hpp       |  8 ++--
 .../linear/KinematicManifoldLQOCTest.cpp      | 20 +++-------
 .../test/solver/linear/ManifoldLQOCTest.cpp   |  5 ++-
 5 files changed, 35 insertions(+), 40 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index f40824a1a..93c0e56a0 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -14,6 +14,7 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
     x_deviation_.setZero();
     x_nominal_.setZero();
     Q_.setZero();
+    Adj_.setIdentity();
     Jl_.setIdentity();
     Jr_.setIdentity();
     u_nominal_.setZero();
@@ -34,6 +35,7 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
 {
     x_deviation_.setZero();
     u_deviation_.setZero();
+    Adj_.setIdentity();
     Jl_.setIdentity();
     Jr_.setIdentity();
 }
@@ -48,6 +50,7 @@ CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::CostFunctionQuadraticSimple(
     : x_deviation_(arg.x_deviation_),
       x_nominal_(arg.x_nominal_),
       Q_(arg.Q_),
+      Adj_(arg.Adj_),
       Jl_(arg.Jl_),
       Jr_(arg.Jr_),
       u_deviation_(arg.u_deviation_),
@@ -73,15 +76,16 @@ void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::setCurrentStateAndContr
     this->u_ = u;
     this->t_ = t;
 
-    this->u_deviation_ = u - u_nominal_;
-    this->x_deviation_ = x_nominal_.rminus(this->x_, Jl_, Jr_);
+    u_deviation_ = u - u_nominal_;
+    x_deviation_ = x_nominal_.rminus(x, Jl_, Jr_);  // ref frame located in x
+
+    Adj_ = (x_deviation_.exp()).adj();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate() -> SCALAR
 {
-    // TODO: including Jr_ might not be required here (identical result)
-    SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Jr_ * Q_ * Jr_.transpose() * x_deviation_)(0);
+    SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Adj_.transpose() * Q_ * Adj_ * x_deviation_)(0);
     SCALAR costR = SCALAR(0.5) * (u_deviation_.transpose() * R_ * u_deviation_)(0);
     return costQ + costR;
 }
@@ -89,13 +93,13 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate()
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
 {
-    return Q_ * Jr_.transpose() * x_deviation_;
+    return -Q_ * Adj_.transpose() * x_deviation_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeIntermediate() -> state_matrix_t
 {
-    return Jr_ * Q_ * Jr_.transpose();
+    return Adj_.transpose() * Q_ * Adj_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -119,30 +123,25 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateControlDerivativeI
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
 {
-    // TODO: including Jr_ might not be required here (identical result)
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
-    // return SCALAR(0.5) * (x_deviation_final.transpose() * Jr * Q_final_ * Jr.transpose() * x_deviation_final)(0);
-    return SCALAR(0.5) * (x_deviation_final.transpose() * Q_final_ * x_deviation_final)(0);
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
+    auto adj = (x_deviation_final.exp()).adj();
+    return SCALAR(0.5) * (x_deviation_final.transpose() * adj.transpose() * Q_final_ * adj * x_deviation_final)(0);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
 {
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
-    return Q_final_ * x_deviation_final;
-    // return Q_final_ * Jr.transpose() * x_deviation_final;
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);  // ref frame located in x_
+    auto adj = (x_deviation_final.exp()).adj();
+    return -Q_final_ * adj.transpose() * x_deviation_final;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;  // TODO: ignore Jl cleanly
-    x_final_.rminus(this->x_, Jl, Jr);
-
-    return Q_final_;
-    // return Jr * Q_final_ * Jr.transpose();
+    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
+    auto adj = (x_deviation_final.exp()).adj();
+    return adj.transpose() * Q_final_ * adj;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
index a912963b7..d9e123e77 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -91,7 +91,8 @@ class CostFunctionQuadraticSimple : public CostFunctionQuadratic<MANIFOLD, CONTR
     typename MANIFOLD::Tangent x_deviation_;
     MANIFOLD x_nominal_;
     state_matrix_t Q_;
-    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl_; // TODO: get rid of Jl cleanly
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Adj_;
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl_;
     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jr_;
 
     control_vector_t u_deviation_;
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
index 52b9686ff..e1faf9ebf 100644
--- a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -63,7 +63,7 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeStatesAndControls()
         this->u_sol_[k] = this->lv_[k] + this->L_[k] * this->x_sol_[k];
 
         //! state update rule in diff coordinates
-        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];                 // A "without trick for backward pass" //TODO: document this
+        StateMatrix A_orig = p.Adj_x_[k + 1] * p.A_[k];  // A "without trick for backward pass" //TODO: document this
         StateControlMatrix B_orig = p.Adj_x_[k + 1] * p.B_[k];          // B "without trick for backward pass"
         typename MANIFOLD::Tangent b_orig = p.Adj_x_[k + 1] * p.b_[k];  // b "without trick for backward pass"
         // Note that we need to transport the state update into the tagent space of k+1
@@ -155,9 +155,9 @@ void GNRiccatiSolver<MANIFOLD, CONTROL_DIM>::computeCostToGo(size_t 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()*/ += G_[k].transpose() * this->lv_[k]; // TODO: bring back all the noalias()
+    sv_[k] /*.noalias()*/ = sv_[k] + p.A_[k].transpose() * sv_[k + 1];
+    sv_[k] /*.noalias()*/ = sv_[k] + p.A_[k].transpose() * S_[k + 1] * p.b_[k];
+    sv_[k] /*.noalias()*/ = sv_[k] + G_[k].transpose() * this->lv_[k];  // TODO: bring back all the noalias()
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
index 27e1ca2d2..fcd9a7322 100644
--- a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
@@ -135,11 +135,11 @@ int main(int argc, char** argv)
     Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
     Eigen::Matrix<double, control_dim, control_dim> R;
     Q_final.setZero();
-    Q_final.diagonal() << 10000, 10000, 10000, 10000, 10000, 10000;
+    //Q_final.diagonal() << 10000, 10000, 10000, 10000, 10000, 10000;
     Q.setZero();
-    //Q.diagonal() << 1, 1, 1, 1, 1, 1;
+    Q.diagonal() << 1, 1, 1, 1, 1, 1;
     R.setZero();
-    R.diagonal() << 300, 300, 300, 300, 300, 300;
+    R.diagonal() << 1, 1, 1, 1, 1, 1;
     ManifoldState_t x_final = manif::SE3<double>(1, 1, 1, 0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
     ManifoldState_t x_nominal = x_final;
@@ -185,19 +185,11 @@ int main(int argc, char** argv)
 
                 problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
 
-                if (idx == 0)  // make the corrections for the standard riccati solver
-                {
-                    problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
-                    problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
-                    problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
-                }
+                problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
+                problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
+                problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
             }
 
-            auto l = x_final.rminus(x_traj.back());
-            auto l_adj = (l.exp()).adj();
-            problems[idx]->Q_.back() = l_adj.transpose() * problems[idx]->Q_.back() * l_adj;
-            problems[idx]->qv_.back().coeffs() = - l_adj.transpose() * problems[idx]->qv_.back().coeffs();
-
             // set the problem pointers
             lqocSolvers[idx]->setProblem(problems[idx]);
 
diff --git a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
index 618146b35..1857ef050 100644
--- a/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/ManifoldLQOCTest.cpp
@@ -262,6 +262,7 @@ class CompositeManifold
             (*Jr).template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = vJr;
         }
 
+        // Das was ich hier als Jacobian verkaufe, was ist das eigentlich?
         return t;
     }
 
@@ -269,7 +270,7 @@ class CompositeManifold
     Tangent log(OptJacobianRef J_t_m = {}) const
     {
         if (J_t_m)
-            throw std::runtime_error("J_t_m compuation not defined.");
+            throw std::runtime_error("J_t_m computation not defined.");
 
         return rminus(CompositeManifold::Identity(), J_t_m);
     }
@@ -279,6 +280,8 @@ class CompositeManifold
         Jacobian J = Jacobian::Zero();
         J.template topLeftCorner<POS_MAN::DoF, POS_MAN::DoF>() = m_pos_.adj();
         J.template bottomRightCorner<VEL_MAN::DoF, VEL_MAN::DoF>() = m_pos_.adj();
+
+// die adjoint hier ist eine reine rotation
         return J;
     }
 

From 755d93a286d2a7f608f878f25a50839e135187c8 Mon Sep 17 00:00:00 2001
From: Markus Giftthaler <markusgft@gmail.com>
Date: Sat, 19 Dec 2020 16:57:00 +0100
Subject: [PATCH 21/21] development backup holidays.

---
 .../CostFunctionQuadraticSimple-impl.hpp      | 51 ++++++++---
 ct_optcon/test/CMakeLists.txt                 |  8 +-
 .../linear/KinematicManifoldLQOCTest.cpp      | 85 ++++++++++++++++---
 3 files changed, 113 insertions(+), 31 deletions(-)

diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
index 93c0e56a0..1098d8469 100644
--- a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -77,15 +77,15 @@ void CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::setCurrentStateAndContr
     this->t_ = t;
 
     u_deviation_ = u - u_nominal_;
-    x_deviation_ = x_nominal_.rminus(x, Jl_, Jr_);  // ref frame located in x
+    x_deviation_ = x.rminus(x_nominal_, Jl_, Jr_);  // ref frame located in x
 
-    Adj_ = (x_deviation_.exp()).adj();
+    Adj_ = x_deviation_.exp().adj();
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate() -> SCALAR
 {
-    SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Adj_.transpose() * Q_ * Adj_ * x_deviation_)(0);
+    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;
 }
@@ -93,13 +93,15 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateIntermediate()
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeIntermediate()
 {
-    return -Q_ * Adj_.transpose() * x_deviation_;
+    // return -Adj_.transpose() * Q_ * Adj_ * x_deviation_;
+    return Jl_.transpose() * Q_ * x_deviation_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeIntermediate() -> state_matrix_t
 {
-    return Adj_.transpose() * Q_ * Adj_;
+    //return Adj_.transpose() * Q_ * Adj_;
+    return Jl_.transpose() * Q_ * Jl_;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
@@ -123,25 +125,46 @@ auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateControlDerivativeI
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::evaluateTerminal() -> SCALAR
 {
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
-    auto adj = (x_deviation_final.exp()).adj();
-    return SCALAR(0.5) * (x_deviation_final.transpose() * adj.transpose() * Q_final_ * adj * x_deviation_final)(0);
+    // Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    // typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    // auto Adj = x_deviation_final.exp().adj();
+    // //return SCALAR(0.5) * (x_deviation_final.transpose() * Jr * Q_final_ * Jr.transpose() * x_deviation_final)(0);
+    // return SCALAR(0.5) * (x_deviation_final.transpose() * Adj * Q_final_ * Adj.transpose() * x_deviation_final)(0);
+
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, Jl, Jr);
+    //auto Adj = x_deviation_final.exp().adj();
+    return SCALAR(0.5) * (x_deviation_final.transpose() * Q_final_ * x_deviation_final)(0);
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 typename MANIFOLD::Tangent CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateDerivativeTerminal()
 {
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);  // ref frame located in x_
-    auto adj = (x_deviation_final.exp()).adj();
-    return -Q_final_ * adj.transpose() * x_deviation_final;
+    //     Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    //     typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    //     auto Adj = x_deviation_final.exp().adj();
+    // //    return -Jr * Q_final_ * Jr.transpose() * x_deviation_final;
+    //     return -Adj * Q_final_ * Adj.transpose() * x_deviation_final;
+
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, Jl, Jr);
+    //auto Adj = x_deviation_final.exp().adj();
+    return Jl.transpose() * Q_final_ * x_deviation_final;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
 auto CostFunctionQuadraticSimple<MANIFOLD, CONTROL_DIM>::stateSecondDerivativeTerminal() -> state_matrix_t
 {
-    typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_);
-    auto adj = (x_deviation_final.exp()).adj();
-    return adj.transpose() * Q_final_ * adj;
+    // Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    // typename MANIFOLD::Tangent x_deviation_final = x_final_.rminus(this->x_, Jl, Jr);
+    // auto Adj = x_deviation_final.exp().adj();
+    // //    return Jr * Q_final_ * Jr.transpose();
+    // return Adj * Q_final_ * Adj.transpose();
+
+    Eigen::Matrix<typename MANIFOLD::Scalar, STATE_DIM, STATE_DIM> Jl, Jr;
+    typename MANIFOLD::Tangent x_deviation_final = this->x_.rminus(x_final_, Jl, Jr);
+    //auto Adj = x_deviation_final.exp().adj().inverse().transpose();
+    return Jl.transpose() * Q_final_ * Jl;
 }
 
 template <typename MANIFOLD, size_t CONTROL_DIM>
diff --git a/ct_optcon/test/CMakeLists.txt b/ct_optcon/test/CMakeLists.txt
index 0067cb119..7418cf247 100644
--- a/ct_optcon/test/CMakeLists.txt
+++ b/ct_optcon/test/CMakeLists.txt
@@ -58,10 +58,10 @@ if(BUILD_TESTS)
     set_target_properties(KinematicManifoldLQOCTest PROPERTIES FOLDER test)
     list(APPEND UNIT_TEST_TARGETS KinematicManifoldLQOCTest)    
     
-    add_executable(ManifoldLQOCTest solver/linear/ManifoldLQOCTest.cpp)
-    target_link_libraries(ManifoldLQOCTest ct_optcon ct_core)
-    set_target_properties(ManifoldLQOCTest PROPERTIES FOLDER test)
-    list(APPEND UNIT_TEST_TARGETS ManifoldLQOCTest)
+    #add_executable(ManifoldLQOCTest solver/linear/ManifoldLQOCTest.cpp)
+    #target_link_libraries(ManifoldLQOCTest ct_optcon ct_core)
+    #set_target_properties(ManifoldLQOCTest PROPERTIES FOLDER test)
+    #list(APPEND UNIT_TEST_TARGETS ManifoldLQOCTest)
 
     if(HPIPM)
         message(STATUS "ct_optcon: building unit tests requiring HPIPM")
diff --git a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
index fcd9a7322..bedf5d3fa 100644
--- a/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
+++ b/ct_optcon/test/solver/linear/KinematicManifoldLQOCTest.cpp
@@ -40,6 +40,49 @@ class DiscrSE3LTITestSystem final : public ct::core::ControlledSystem<ManifoldSt
     //ManifoldState_t m_ref_;
 };
 
+void testCoordinateTransformCost()
+{
+    using namespace manif;
+    for (size_t i = 0; i < 3; i++)
+    {
+        SE3d X, Xf;
+        X.setRandom();
+        Xf.setRandom();
+        Eigen::Matrix<double, 6, 6> Jl_tau, Jr_tau, Jl_zet, Jr_zet;
+        auto tau = Xf.rminus(X, Jl_tau, Jr_tau);
+        auto zet = X.rminus(Xf, Jl_zet, Jr_zet);
+        auto Adj_tau = tau.exp().adj();
+        auto Adj_zet = zet.exp().adj();
+        // std::cout << Adj_tau.transpose() * Adj_tau << std::endl;
+        auto tau_recon = -Adj_tau * zet;
+        auto tau_recon_J = Jr_tau * zet;
+        std::cout << "tau:" << tau << std::endl;
+        std::cout << "-Adj_tau * zet:" << std::endl << tau_recon.transpose() << std::endl;
+        std::cout << "Jr_tau * zet:" << std::endl << tau_recon_J.transpose() << std::endl;
+        std::cout << "-Jl_zet * zet:" << std::endl << (-Jl_zet * zet).transpose() << std::endl;
+        std::cout << "zet:" << zet << std::endl;
+        auto zet_recon = -Adj_zet * tau;
+        auto zet_recon_J = Jr_zet * tau;
+        std::cout << "-Adj_zet * tau:" << std::endl << zet_recon.transpose() << std::endl;
+        std::cout << "Jr_zet * tau:" << std::endl << zet_recon_J.transpose() << std::endl;
+        std::cout << "-Jl_tau * tau:" << std::endl << (-Jl_tau * tau).transpose() << std::endl;
+        // construct a random symmetric matrix Q
+        Eigen::Matrix<double, 6, 6> Q, A;
+        A.setRandom();
+        Q = A + A.transpose();
+        // these costs all evaluate the same scalar value, so that means, for the second-order term cost eval we are fine!
+        // we note, however, that the hessian matrices themselves are not identical!!!!
+        std::cout << tau.transpose() * Q * tau << std::endl;
+        std::cout << zet.transpose() * Q * zet << std::endl;
+        std::cout << tau.transpose() * Adj_zet.transpose() * Q * Adj_zet * tau << std::endl;
+        std::cout << tau.transpose() * Jr_zet.transpose() * Q * Jr_zet * tau << std::endl;
+        std::cout << std::endl;
+        // .. but those matrices are not identical:
+        // std::cout << Adj_zet.transpose() * Q * Adj_zet << std::endl;
+        // std::cout << Jr_zet.transpose() * Q * Jr_zet << std::endl;
+        std::cout << std::endl;
+    }
+}
 
 // TODO: make this a unit test
 void testParallelTransport()
@@ -78,16 +121,17 @@ void testParallelTransport()
     // std::cout << m_mpi_2.adj() * b << std::endl << std::endl;
 }
 
-int main(int argc, char** argv)
+
+void ocqpTest()
 {
     std::cout << std::fixed;
 
     const bool use_single_shooting = true;  // toggle between single and multiple shooting
 
-    const size_t N = 150;
-    const double dt = 0.01;
+    const size_t N = 50;
+    const double dt = 0.1;
 
-    const ManifoldState_t x0 = manif::SE3<double>(0, 0, 0, 3.14, 0, 0);
+    const ManifoldState_t x0 = manif::SE3<double>(0, 0, 0, 3.0, 0, 0);
     ct::core::DiscreteArray<ManifoldState_t> x_traj(N + 1, x0);  // init state trajectory, will be overwritten
     ct::core::DiscreteArray<ManifoldState_t::Tangent> b(
         N + 1, ManifoldState_t::Tangent::Zero());                             // defect traj, will be overwritten
@@ -99,7 +143,7 @@ int main(int argc, char** argv)
     // TODO: numerical trouble for more aggressive distributions, since the approximation of the value function becomes really bad?
     for (size_t i = 1; i < N + 1; i++)
     {
-        x_traj[i] = ManifoldState_t::Random();
+        x_traj[i] = x0;  // ManifoldState_t::Random();
     }
 
     // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
@@ -132,14 +176,19 @@ int main(int argc, char** argv)
 
 
     // create a cost function
-    Eigen::Matrix<double, state_dim, state_dim> Q, Q_final;
+    Eigen::Matrix<double, state_dim, state_dim> Q, Q_final, Q_temp;
     Eigen::Matrix<double, control_dim, control_dim> R;
+    Q_temp.setRandom();
     Q_final.setZero();
-    //Q_final.diagonal() << 10000, 10000, 10000, 10000, 10000, 10000;
     Q.setZero();
+    Q_final.diagonal() << 1000, 1000, 1000, 1000, 1000, 1000;
+    Q_final = Q_temp.transpose() * Q_final * Q_temp; 
+    // std::cout << Q_final.eigenvalues() << std::endl;
+    // std::cout << Q_final << std::endl;
     Q.diagonal() << 1, 1, 1, 1, 1, 1;
+    Q = Q_temp.transpose() * Q * Q_temp;
     R.setZero();
-    R.diagonal() << 1, 1, 1, 1, 1, 1;
+    R.diagonal() << 10, 10, 10, 10, 10, 10;
     ManifoldState_t x_final = manif::SE3<double>(1, 1, 1, 0, 0, 0);
     std::cout << "desired final state: " << x_final << std::endl;
     ManifoldState_t x_nominal = x_final;
@@ -166,7 +215,7 @@ int main(int argc, char** argv)
         // std::cout << std::setprecision(4) << "m: " << x_curr << "\t tan: " << x_curr.log() << std::endl;
     }
 
-    size_t nIter = 5;
+    size_t nIter = 20;
     for (size_t iter = 0; iter < nIter; iter++)
     {
         // initialize the optimal control problems for both solvers
@@ -180,14 +229,17 @@ int main(int argc, char** argv)
             // dynamics transportation
             for (size_t i = 0; i < N; i++)
             {
-                auto l = x_traj[i + 1].rminus(x_traj[i]);
+                Eigen::Matrix<double, state_dim, state_dim> Jl, Jr;
+                auto l = x_traj[i + 1].rminus(x_traj[i], Jl, Jr);
                 auto l_adj = (l.exp()).adj();
 
+                auto m = Jl;  // best guess: Jl
+
                 problems[idx]->Adj_x_[i + 1] = l_adj;  // parallel transport matrix / adjoint from stage k+1 to stage k
 
-                problems[idx]->A_[i] = l_adj.transpose() * problems[idx]->A_[i];
-                problems[idx]->B_[i] = l_adj.transpose() * problems[idx]->B_[i];
-                problems[idx]->b_[i] = l_adj.transpose() * problems[idx]->b_[i];
+                problems[idx]->A_[i] = m.transpose() * problems[idx]->A_[i];
+                problems[idx]->B_[i] = m.transpose() * problems[idx]->B_[i];
+                problems[idx]->b_[i] = m.transpose() * problems[idx]->b_[i];
             }
 
             // set the problem pointers
@@ -335,5 +387,12 @@ int main(int argc, char** argv)
     }
     EigenFileExport::mat_to_file(EigenFileExport::CSVFormat(), "/tmp/rot_traj.csv", rot_traj);
     EigenFileExport::mat_to_file(EigenFileExport::CSVFormat(), "/tmp/trans_traj.csv", trans_traj);
+}
+
+
+int main(int argc, char** argv)
+{
+    //testCoordinateTransformCost();
+    ocqpTest();
     return 1;
 }
\ No newline at end of file