AD_ContinuousOptCon.cpp

/*!
 * Simple example how to linearize a system and design an LQR controller.
 *
 * \example LQR.cpp
 */


#include <ct/optcon/optcon.h>  // also includes ct_core


int main(int argc, char** argv)
{
    // get the state and control input dimension of the oscillator
    const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
    const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;

    // create an auto-differentiable instance of the oscillator dynamics
    ct::core::ADCGScalar w_n(50.0);
    std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, ct::core::ADCGScalar>> oscillatorDynamics(
        new ct::core::tpl::SecondOrderSystem<ct::core::ADCGScalar>(w_n));

    // create an Auto-Differentiation Linearizer with code generation on the quadrotor model (shared_ptr)
    std::shared_ptr<ct::core::ADCodegenLinearizer<state_dim, control_dim>> adLinearizer(new ct::core::ADCodegenLinearizer<state_dim, control_dim>(oscillatorDynamics));
 

    // compile the linearized model just-in-time
    adLinearizer->compileJIT();

    // define the linearization point around steady state
    ct::core::StateVector<state_dim> x;
    x.setZero();
    ct::core::ControlVector<control_dim> u;
    u.setZero();
    double t = 0.0;

    // compute the linearization around the nominal state using the Auto-Diff Linearizer
    auto A = adLinearizer->getDerivativeState(x, u, t);
    auto B = adLinearizer->getDerivativeControl(x, u, t);

    // load the weighting matrices
    ct::optcon::TermQuadratic<state_dim, control_dim> quadraticCost;
    ct::core::StateMatrix<state_dim> Q = ct::core::StateMatrix<state_dim>::Identity();
    ct::core::ControlMatrix<control_dim> R = ct::core::ControlMatrix<control_dim>::Identity();

    // design the LQR controller
    ct::optcon::LQR<state_dim, control_dim> lqrSolver;
    ct::core::FeedbackMatrix<state_dim, control_dim> K;

    std::cout << "A: " << std::endl << A << std::endl << std::endl;
    std::cout << "B: " << std::endl << B << std::endl << std::endl;
    std::cout << "Q: " << std::endl << Q << std::endl << std::endl;
    std::cout << "R: " << std::endl << R << std::endl << std::endl;

    lqrSolver.compute(Q, R, A, B, K);

    std::cout << "LQR gain matrix:" << std::endl << K << std::endl;

    ct::core::StateVector<state_dim> x0;
    ct::core::Time timeHorizon = 1.0;
    std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
                new ct::optcon::TermQuadratic<state_dim, control_dim>());
            std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
                new ct::optcon::TermQuadratic<state_dim, control_dim>());

    intermediateCost->setWeights(Q, R);

    std::shared_ptr<ct::optcon::CostFunctionQuadratic<state_dim, control_dim>> costFunction(
        new ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
    costFunction->addIntermediateTerm(intermediateCost);

    ct::optcon::ContinuousOptConProblem<state_dim, control_dim, ct::core::ADCGScalar> optConProblem(
        timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);


    return 1;
}

Hi @tsaoyu, now I believe I know your issue.
the problem is that you need to have TWO instances of your dynamics. One for the AD linearizer, and one as a regular double instance, which gets handed to the optimal control problem.
So, just create a second version, templated on double ct::core::ControlledSystem<state_dim, control_dim, double>, and pass that one in line 72 instead of the oscillatorDynamics.

@markusgft got it, two instances of dynamics did solve the problem.

A working example:

/*!
 * Simple example how to linearize a system and design an LQR controller.
 *
 * \example LQR.cpp
 */


#include <ct/optcon/optcon.h>  // also includes ct_core


int main(int argc, char** argv)
{
    // get the state and control input dimension of the oscillator
    const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
    const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;

    // create an auto-differentiable instance of the oscillator dynamics
    ct::core::ADCGScalar w_n(50.0);
    std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, ct::core::ADCGScalar>> oscillatorDynamics(
        new ct::core::tpl::SecondOrderSystem<ct::core::ADCGScalar>(w_n));

    std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamicsOpt(
        new ct::core::SecondOrderSystem(50.0));

    // create an Auto-Differentiation Linearizer with code generation on the quadrotor model (shared_ptr)
    std::shared_ptr<ct::core::ADCodegenLinearizer<state_dim, control_dim>> adLinearizer(new ct::core::ADCodegenLinearizer<state_dim, control_dim>(oscillatorDynamics));
 

    // compile the linearized model just-in-time
    adLinearizer->compileJIT();

    // define the linearization point around steady state
    ct::core::StateVector<state_dim> x;
    x.setZero();
    ct::core::ControlVector<control_dim> u;
    u.setZero();
    double t = 0.0;

    // compute the linearization around the nominal state using the Auto-Diff Linearizer
    auto A = adLinearizer->getDerivativeState(x, u, t);
    auto B = adLinearizer->getDerivativeControl(x, u, t);

    // load the weighting matrices
    ct::optcon::TermQuadratic<state_dim, control_dim> quadraticCost;
    ct::core::StateMatrix<state_dim> Q = ct::core::StateMatrix<state_dim>::Identity();
    ct::core::ControlMatrix<control_dim> R = ct::core::ControlMatrix<control_dim>::Identity();

    // design the LQR controller
    ct::optcon::LQR<state_dim, control_dim> lqrSolver;
    ct::core::FeedbackMatrix<state_dim, control_dim> K;

    std::cout << "A: " << std::endl << A << std::endl << std::endl;
    std::cout << "B: " << std::endl << B << std::endl << std::endl;
    std::cout << "Q: " << std::endl << Q << std::endl << std::endl;
    std::cout << "R: " << std::endl << R << std::endl << std::endl;

    lqrSolver.compute(Q, R, A, B, K);

    std::cout << "LQR gain matrix:" << std::endl << K << std::endl;

    ct::core::StateVector<state_dim> x0;
    ct::core::Time timeHorizon = 1.0;
    std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
                new ct::optcon::TermQuadratic<state_dim, control_dim>());
            std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
                new ct::optcon::TermQuadratic<state_dim, control_dim>());

    intermediateCost->setWeights(Q, R);

    std::shared_ptr<ct::optcon::CostFunctionQuadratic<state_dim, control_dim>> costFunction(
        new ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
    costFunction->addIntermediateTerm(intermediateCost);

    ct::optcon::ContinuousOptConProblem<state_dim, control_dim> optConProblem(
        timeHorizon, x0, oscillatorDynamicsOpt, costFunction, adLinearizer);


    return 1;
}