DerivativesCppadCG.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>
#include "DerivativesCppadSettings.h"

namespace ct {
namespace core {

#ifdef CPPADCG


template <int IN_DIM, int OUT_DIM>
class DerivativesCppadCG
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ADCGScalar CG_SCALAR;         
    typedef ADCGValueType CG_VALUE_TYPE;  

    typedef Eigen::Matrix<CG_SCALAR, IN_DIM, 1> IN_TYPE_CG;    
    typedef Eigen::Matrix<CG_SCALAR, OUT_DIM, 1> OUT_TYPE_CG;  
    typedef Eigen::Matrix<bool, IN_DIM, OUT_DIM> Sparsity;     
    typedef Eigen::Matrix<bool, IN_DIM, IN_DIM> HessianSparsity;

    typedef std::function<OUT_TYPE_CG(const IN_TYPE_CG&)> FUN_TYPE_CG;  


    DerivativesCppadCG(FUN_TYPE_CG& f, int inputDim = IN_DIM, int outputDim = OUT_DIM)
        : cgStdFun_(f), inputDim_(inputDim), outputDim_(outputDim), tmpVarCount_(0)
    {
        if (outputDim > 0 && inputDim > 0)
            this->recordCg();
    }

    DerivativesCppadCG(const DerivativesCppadCG& arg)
        : cgStdFun_(arg.cgStdFun_), inputDim_(arg.inputDim_), outputDim_(arg.outputDim_), tmpVarCount_(arg.tmpVarCount_)
    {
    }

    virtual ~DerivativesCppadCG() = default;
    DerivativesCppadCG* clone() const { return new DerivativesCppadCG<IN_DIM, OUT_DIM>(*this); }

    void generateJacobianSource(const std::string& derivativeName,
        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",
        const Sparsity& sparsity = Sparsity::Ones(),
        bool useReverse = true,
        bool ignoreZero = true)
    {
        internal::SparsityPattern pattern;
        pattern.initPattern(sparsity);

        size_t jacDimension = IN_DIM * OUT_DIM;

        std::string codeJac = internal::CGHelpers::generateJacobianSource(
            cgCppadFun_, pattern, jacDimension, tmpVarCount_, useReverse, ignoreZero);

        writeCodeFile(templateDir, "/Jacobian.tpl.h", "/Jacobian.tpl.cpp", outputDir, derivativeName, ns1, ns2, codeJac,
            "AUTOGENERATED_CODE_PLACEHOLDER");
    }


    void generateForwardZeroSource(const std::string& forwardZeroName,
        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 ignoreZero = true)
    {
        std::string codeJac = internal::CGHelpers::generateForwardZeroSource(cgCppadFun_, tmpVarCount_, ignoreZero);

        writeCodeFile(templateDir, "/ForwardZero.tpl.h", "/ForwardZero.tpl.cpp", outputDir, forwardZeroName, ns1, ns2,
            codeJac, "AUTOGENERATED_CODE_PLACEHOLDER");
    }


    void generateHessianSource(const std::string& derivativeName,
        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",
        const HessianSparsity& sparsity = HessianSparsity::Ones(),
        bool useReverse = true,
        bool ignoreZero = true)
    {
        internal::SparsityPattern pattern;
        pattern.initPattern(sparsity);

        size_t hesDimension = IN_DIM * IN_DIM;

        std::string codeHes =
            internal::CGHelpers::generateHessianSource(cgCppadFun_, pattern, hesDimension, tmpVarCount_, ignoreZero);

        writeCodeFile(templateDir, "/Hessian.tpl.h", "/Hessian.tpl.cpp", outputDir, derivativeName, ns1, ns2, codeHes,
            "AUTOGENERATED_CODE_PLACEHOLDER");
    }


private:
    void recordCg()
    {
        // input vector, needs to be dynamic size
        Eigen::Matrix<CG_SCALAR, Eigen::Dynamic, 1> x(inputDim_);
        // init to rand to avoid floating point problems in user's code
        x.setRandom();

        // declare x as independent
        CppAD::Independent(x);

        // output vector, needs to be dynamic size
        Eigen::Matrix<CG_SCALAR, Eigen::Dynamic, 1> y(outputDim_);

        y = cgStdFun_(x);

        // store operation sequence in f: x -> y and stop recording
        CppAD::ADFun<CG_VALUE_TYPE> fCodeGen(x, y);

        fCodeGen.optimize();

        cgCppadFun_ = fCodeGen;
    }


    void writeCodeFile(const std::string& templateDir,
        const std::string& tplHeaderName,
        const std::string& tplSourceName,
        const std::string& outputDir,
        const std::string& derivativeName,
        const std::string& ns1,
        const std::string& ns2,
        const std::string& codeJac,
        const std::string& codePlaceholder)
    {
        std::cout << "Writing code of " + derivativeName + " to file..." << std::endl;

        std::string header = internal::CGHelpers::parseFile(templateDir + tplHeaderName);
        std::string source = internal::CGHelpers::parseFile(templateDir + tplSourceName);

        replaceSizesAndNames(header, derivativeName, ns1, ns2);
        replaceSizesAndNames(source, derivativeName, ns1, ns2);

        internal::CGHelpers::replaceOnce(header, "MAX_COUNT", std::to_string(tmpVarCount_));
        internal::CGHelpers::replaceOnce(source, codePlaceholder, codeJac);


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


        std::cout << "... Done! Successfully generated " + derivativeName << std::endl;
    }



    void replaceSizesAndNames(std::string& file,
        const std::string& systemName,
        const std::string& ns1,
        const std::string& ns2)
    {
        internal::CGHelpers::replaceAll(file, "DERIVATIVE_NAME", systemName);
        internal::CGHelpers::replaceAll(file, "NS1", ns1);
        internal::CGHelpers::replaceAll(file, "NS2", ns2);
        internal::CGHelpers::replaceAll(file, "IN_DIM", std::to_string(IN_DIM));
        internal::CGHelpers::replaceAll(file, "OUT_DIM", std::to_string(OUT_DIM));
    }

    std::function<OUT_TYPE_CG(const IN_TYPE_CG&)> cgStdFun_;  

    int inputDim_;   
    int outputDim_;  

    CppAD::ADFun<CG_VALUE_TYPE> cgCppadFun_;  

    size_t tmpVarCount_;  
};

#endif

} /* namespace core */
} /* namespace ct */