ADCodegenLinearizer.h

Go to the documentation of this file.

/**********************************************************************************************************************
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 <ct/core/templateDir.h>

#ifdef CPPADCG

namespace ct {
namespace core {


template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class ADCodegenLinearizer : public LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;  

    typedef CppAD::AD<CppAD::cg::CG<SCALAR>> ADCGScalar;  

    typedef ControlledSystem<STATE_DIM, CONTROL_DIM, ADCGScalar> system_t;  
    typedef DynamicsLinearizerADCG<STATE_DIM, CONTROL_DIM, ADCGScalar, ADCGScalar>
        linearizer_t;  

    typedef typename Base::state_vector_t state_vector_t;                  
    typedef typename Base::control_vector_t control_vector_t;              
    typedef typename Base::state_matrix_t state_matrix_t;                  
    typedef typename Base::state_control_matrix_t state_control_matrix_t;  



    ADCodegenLinearizer(std::shared_ptr<system_t> nonlinearSystem, bool cacheJac = true)
        : Base(nonlinearSystem->getType()),
          dFdx_(state_matrix_t::Zero()),
          dFdu_(state_control_matrix_t::Zero()),
          cacheJac_(cacheJac),
          nonlinearSystem_(nonlinearSystem),
          linearizer_(std::bind(&system_t::computeControlledDynamics,
              nonlinearSystem_.get(),
              std::placeholders::_1,
              std::placeholders::_2,
              std::placeholders::_3,
              std::placeholders::_4))
    {
    }

    ADCodegenLinearizer(const ADCodegenLinearizer<STATE_DIM, CONTROL_DIM>& arg)
        : Base(arg.nonlinearSystem_->getType()),
          dFdx_(arg.dFdx_),
          dFdu_(arg.dFdu_),
          cacheJac_(arg.cacheJac_),
          nonlinearSystem_(arg.nonlinearSystem_->clone()),
          linearizer_(arg.linearizer_)
    {
    }

    ADCodegenLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
    {
        return new ADCodegenLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
    }


    const state_matrix_t& getDerivativeState(const state_vector_t& x,
        const control_vector_t& u,
        const SCALAR t = SCALAR(0.0)) override
    {
        dFdx_ = linearizer_.getDerivativeState(x, u, t);
        return dFdx_;
    }


    const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const SCALAR t = SCALAR(0.0)) override
    {
        dFdu_ = linearizer_.getDerivativeControl(x, u, t);
        return dFdu_;
    }


    void compileJIT(const std::string& libName = "ADCodegenLinearizer") { linearizer_.compileJIT(libName); }

    void generateCode(const std::string& systemName,
        const std::string& outputDir = ct::core::CODEGEN_OUTPUT_DIR,
        const std::string& templateDir = ct::core::CODEGEN_TEMPLATE_DIR,
        const std::string& ns1 = "core",
        const std::string& ns2 = "generated",
        bool useReverse = false,
        bool ignoreZero = true)
    {
        std::string codeJacA, codeJacB;
        linearizer_.generateCode(codeJacA, codeJacB, useReverse, ignoreZero);

        writeCodeFile(
            templateDir, outputDir, systemName, ns1, ns2, codeJacA, codeJacB, "AUTOGENERATED_CODE_PLACEHOLDER");
    }

    const linearizer_t& getLinearizer() const { return linearizer_; }
private:

    void writeCodeFile(const std::string& templateDir,
        const std::string& outputDir,
        const std::string& systemName,
        const std::string& ns1,
        const std::string& ns2,
        const std::string& codeJacA,
        const std::string& codeJacB,
        const std::string& codePlaceholder)
    {
        std::cout << "Generating linear system..." << std::endl;

        size_t maxTempVarCountState, maxTempVarCountControl;
        linearizer_.getMaxTempVarCount(maxTempVarCountState, maxTempVarCountControl);

        std::string header = internal::CGHelpers::parseFile(templateDir + "/LinearSystem.tpl.h");
        std::string source = internal::CGHelpers::parseFile(templateDir + "/LinearSystem.tpl.cpp");

        const std::string scalarName(linearizer_.getOutScalarType());

        replaceSizesAndNames(header, systemName, scalarName, ns1, ns2);
        replaceSizesAndNames(source, systemName, scalarName, ns1, ns2);

        internal::CGHelpers::replaceOnce(header, "MAX_COUNT_STATE", std::to_string(maxTempVarCountState));
        internal::CGHelpers::replaceOnce(header, "MAX_COUNT_CONTROL", std::to_string(maxTempVarCountControl));

        internal::CGHelpers::replaceOnce(source, codePlaceholder + "_JAC_A", codeJacA);
        internal::CGHelpers::replaceOnce(source, codePlaceholder + "_JAC_B", codeJacB);

        internal::CGHelpers::writeFile(outputDir + "/" + systemName + ".h", header);
        internal::CGHelpers::writeFile(outputDir + "/" + systemName + ".cpp", source);


        std::cout << "... Done! Successfully generated linear system" << std::endl;
    }


    void replaceSizesAndNames(std::string& file,
        const std::string& systemName,
        const std::string& scalarName,
        const std::string& ns1,
        const std::string& ns2)
    {
        internal::CGHelpers::replaceAll(file, "LINEAR_SYSTEM_NAME", systemName);
        internal::CGHelpers::replaceAll(file, "NS1", ns1);
        internal::CGHelpers::replaceAll(file, "NS2", ns2);
        internal::CGHelpers::replaceAll(file, "STATE_DIM", std::to_string(STATE_DIM));
        internal::CGHelpers::replaceAll(file, "CONTROL_DIM", std::to_string(CONTROL_DIM));
        internal::CGHelpers::replaceAll(file, "SCALAR", scalarName);
    }

    state_matrix_t dFdx_;          
    state_control_matrix_t dFdu_;  

    bool cacheJac_;  

    std::shared_ptr<system_t> nonlinearSystem_;  

    linearizer_t linearizer_;  
};

}  // namespace core
}  // namespace ct

#endif