From 7f91843c227ed223cabfd9cebced39b312454b2e Mon Sep 17 00:00:00 2001 From: Patrick Peltzer Date: Thu, 25 Jun 2020 20:33:52 +0200 Subject: [PATCH] Added prototype CostFunctionADParams class showcasing how to use dynamic parameters in a CppAD cost function (parameters passive w.r.t. differentiation). Rather an ugly hack to use as a starting point to add proper support. --- .../CostFunctionADParams-impl.hpp | 319 ++++++++++++++++++ .../costfunction/CostFunctionADParams.hpp | 168 +++++++++ 2 files changed, 487 insertions(+) create mode 100644 ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams-impl.hpp create mode 100644 ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams.hpp diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams-impl.hpp new file mode 100644 index 000000000..4f4e3e322 --- /dev/null +++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams-impl.hpp @@ -0,0 +1,319 @@ +#pragma once + +#ifdef CPPADCG + +namespace ct { +namespace optcon { + +template +CostFunctionADParams::CostFunctionADParams() + : CostFunctionQuadratic(), + stateParamsControlTime_(Eigen::Matrix::Zero()) +{ + intermediateFun_ = [&](const Eigen::Matrix& stateParamsInputTime) { + return this->evaluateIntermediateCg(stateParamsInputTime); + }; + + finalFun_ = [&](const Eigen::Matrix& stateParamsInputTime) { + return this->evaluateTerminalCg(stateParamsInputTime); + }; + + intermediateCostCodegen_ = std::shared_ptr(new JacCG(intermediateFun_, STATE_DIM + CONTROL_DIM + 1 + PARAMS_DIM, 1)); + finalCostCodegen_ = std::shared_ptr(new JacCG(finalFun_, STATE_DIM + CONTROL_DIM + 1 + PARAMS_DIM, 1)); + + setCurrentStateAndControl(this->x_, this->u_, this->t_); +} + +template +CostFunctionADParams::CostFunctionADParams(const CostFunctionADParams& arg) + : CostFunctionQuadratic(arg), + stateParamsControlTime_(arg.stateParamsControlTime_), + intermediateFun_(arg.intermediateFun_), + finalFun_(arg.finalFun_) +{ + intermediateTerms_.resize(arg.intermediateTerms_.size()); + intermediateTermsParam_.resize(arg.intermediateTermsParam_.size()); + finalTerms_.resize(arg.finalTerms_.size()); + finalTermsParam_.resize(arg.finalTermsParam_.size()); + + for (size_t i = 0; i < intermediateTerms_.size(); ++i) + intermediateTerms_[i] = + std::shared_ptr>(arg.intermediateTerms_[i]->clone()); + + for (size_t i = 0; i < intermediateTermsParam_.size(); ++i) + intermediateTermsParam_[i] = + std::shared_ptr>(arg.intermediateTermsParam_[i]->clone()); + + for (size_t i = 0; i < finalTerms_.size(); ++i) + finalTerms_[i] = + std::shared_ptr>(arg.finalTerms_[i]->clone()); + + for (size_t i = 0; i < finalTermsParam_.size(); ++i) + finalTermsParam_[i] = + std::shared_ptr>(arg.finalTermsParam_[i]->clone()); + + intermediateCostCodegen_ = std::shared_ptr(arg.intermediateCostCodegen_->clone()); + finalCostCodegen_ = std::shared_ptr(arg.finalCostCodegen_->clone()); +} + +template +CostFunctionADParams::~CostFunctionADParams() +{ +} + +template +CostFunctionADParams* CostFunctionADParams::clone() const +{ + return new CostFunctionADParams(*this); +} + +template +void CostFunctionADParams::initialize() +{ + intermediateFun_ = [&](const Eigen::Matrix& stateParamsInputTime) { + return this->evaluateIntermediateCg(stateParamsInputTime); + }; + + finalFun_ = [&](const Eigen::Matrix& stateParamsInputTime) { + return this->evaluateTerminalCg(stateParamsInputTime); + }; + + intermediateCostCodegen_->update(intermediateFun_, STATE_DIM + CONTROL_DIM + 1 + PARAMS_DIM, 1); + finalCostCodegen_->update(finalFun_, STATE_DIM + CONTROL_DIM + 1 + PARAMS_DIM, 1); + + //! @ todo: this should probably become an option (eg. IPOPT can work without cost Hessians) + ct::core::DerivativesCppadSettings settings; + settings.createForwardZero_ = true; + settings.createJacobian_ = true; + settings.createHessian_ = true; + + finalCostCodegen_->compileJIT(settings, "finalCosts"); + intermediateCostCodegen_->compileJIT(settings, "intermediateCosts"); +} + +template +void CostFunctionADParams::addIntermediateADTerm( + std::shared_ptr> term, + bool verbose) +{ + intermediateTerms_.push_back(term); + + if (verbose) + { + std::cout << term->getName() + " added as intermediate AD term" << std::endl; + } +} + +template +void CostFunctionADParams::addIntermediateADTermParam( + std::shared_ptr> term, + bool verbose) +{ + intermediateTermsParam_.push_back(term); + + if (verbose) + { + std::cout << term->getName() + " added as intermediate AD term with passive parameters" << std::endl; + } +} + +template +void CostFunctionADParams::addFinalADTerm( + std::shared_ptr> term, + bool verbose) +{ + finalTerms_.push_back(term); + + if (verbose) + { + std::cout << term->getName() + " added as final AD term" << std::endl; + } +} + +template +void CostFunctionADParams::addFinalADTermParam( + std::shared_ptr> term, + bool verbose) +{ + finalTermsParam_.push_back(term); + + if (verbose) + { + std::cout << term->getName() + " added as final AD term with passive parameters" << std::endl; + } +} + +// set state and control +template +void CostFunctionADParams::setCurrentStateAndControl(const state_vector_t& x, + const control_vector_t& u, + const SCALAR& t) +{ + this->x_ = x; + this->u_ = u; + this->t_ = t; + + stateParamsControlTime_.segment(0, STATE_DIM) = x; + stateParamsControlTime_.segment(STATE_DIM+PARAMS_DIM, CONTROL_DIM) = u; + stateParamsControlTime_(STATE_DIM + PARAMS_DIM + CONTROL_DIM) = t; + //stateParamsControlTime_ << x, u, t; +} + +template +void CostFunctionADParams::setCurrentParameters(const params_vector_t& p) { + stateParamsControlTime_.segment(STATE_DIM, PARAMS_DIM) = p; +} + +template +typename CostFunctionADParams::MatrixCg +CostFunctionADParams::evaluateIntermediateCg( + const Eigen::Matrix& stateParamsInputTime) +{ + CGScalar y = CGScalar(0.0); + + for (auto it : intermediateTerms_) + y += it->evaluateCppadCg(stateParamsInputTime.segment(0, STATE_DIM), stateParamsInputTime.segment(STATE_DIM+PARAMS_DIM, CONTROL_DIM), + stateParamsInputTime(STATE_DIM + PARAMS_DIM + CONTROL_DIM)); + + for (auto it : intermediateTermsParam_) + y += it->evaluateCppadCg(stateParamsInputTime.segment(0, STATE_DIM+PARAMS_DIM), stateParamsInputTime.segment(STATE_DIM+PARAMS_DIM, CONTROL_DIM), + stateParamsInputTime(STATE_DIM + PARAMS_DIM + CONTROL_DIM)); + + + Eigen::Matrix out; + out << y; + return out; +} + +template +typename CostFunctionADParams::MatrixCg +CostFunctionADParams::evaluateTerminalCg( + const Eigen::Matrix& stateParamsInputTime) +{ + CGScalar y = CGScalar(0.0); + + for (auto it : finalTerms_) + y += it->evaluateCppadCg(stateParamsInputTime.segment(0, STATE_DIM), stateParamsInputTime.segment(STATE_DIM+PARAMS_DIM, CONTROL_DIM), + stateParamsInputTime(STATE_DIM + PARAMS_DIM + CONTROL_DIM)); + + for (auto it : finalTermsParam_) + y += it->evaluateCppadCg(stateParamsInputTime.segment(0, STATE_DIM+PARAMS_DIM), stateParamsInputTime.segment(STATE_DIM+PARAMS_DIM, CONTROL_DIM), + stateParamsInputTime(STATE_DIM + PARAMS_DIM + CONTROL_DIM)); + + Eigen::Matrix out; + out << y; + return out; +} + +template +SCALAR CostFunctionADParams::evaluateIntermediate() +{ + return this->evaluateIntermediateBase() + intermediateCostCodegen_->forwardZero(stateParamsControlTime_)(0); +} + +template +SCALAR CostFunctionADParams::evaluateTerminal() +{ + return this->evaluateTerminalBase() + finalCostCodegen_->forwardZero(stateParamsControlTime_)(0); +} + +template +typename CostFunctionADParams::state_vector_t +CostFunctionADParams::stateDerivativeIntermediate() +{ + Eigen::Matrix jacTot = + intermediateCostCodegen_->jacobian(stateParamsControlTime_); + return jacTot.template leftCols().transpose() + this->stateDerivativeIntermediateBase(); +} + +template +typename CostFunctionADParams::state_vector_t +CostFunctionADParams::stateDerivativeTerminal() +{ + Eigen::Matrix jacTot = finalCostCodegen_->jacobian(stateParamsControlTime_); + return jacTot.template leftCols().transpose() + this->stateDerivativeTerminalBase(); +} + +template +typename CostFunctionADParams::control_vector_t +CostFunctionADParams::controlDerivativeIntermediate() +{ + Eigen::Matrix jacTot = + intermediateCostCodegen_->jacobian(stateParamsControlTime_); + return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM+PARAMS_DIM).transpose() + this->controlDerivativeIntermediateBase(); +} + +template +typename CostFunctionADParams::control_vector_t +CostFunctionADParams::controlDerivativeTerminal() +{ + Eigen::Matrix jacTot = finalCostCodegen_->jacobian(stateParamsControlTime_); + return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM+PARAMS_DIM).transpose() + this->controlDerivativeTerminalBase(); +} + +template +typename CostFunctionADParams::state_matrix_t +CostFunctionADParams::stateSecondDerivativeIntermediate() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = intermediateCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(0, 0) + this->stateSecondDerivativeIntermediateBase(); +} + +template +typename CostFunctionADParams::state_matrix_t +CostFunctionADParams::stateSecondDerivativeTerminal() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = finalCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(0, 0) + this->stateSecondDerivativeTerminalBase(); +} + +template +typename CostFunctionADParams::control_matrix_t +CostFunctionADParams::controlSecondDerivativeIntermediate() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = intermediateCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(STATE_DIM+PARAMS_DIM, STATE_DIM+PARAMS_DIM) + + this->controlSecondDerivativeIntermediateBase(); +} + +template +typename CostFunctionADParams::control_matrix_t +CostFunctionADParams::controlSecondDerivativeTerminal() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = finalCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(STATE_DIM+PARAMS_DIM, STATE_DIM+PARAMS_DIM) + + this->controlSecondDerivativeTerminalBase(); +} + +template +typename CostFunctionADParams::control_state_matrix_t +CostFunctionADParams::stateControlDerivativeIntermediate() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = intermediateCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(STATE_DIM+PARAMS_DIM, 0) + this->stateControlDerivativeIntermediateBase(); +} + +template +typename CostFunctionADParams::control_state_matrix_t +CostFunctionADParams::stateControlDerivativeTerminal() +{ + Eigen::Matrix w; + w << SCALAR(1.0); + MatrixXs hesTot = finalCostCodegen_->hessian(stateParamsControlTime_, w); + return hesTot.template block(STATE_DIM+PARAMS_DIM, 0) + this->stateControlDerivativeTerminalBase(); +} + +} // namespace optcon +} // namespace ct + +#endif diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams.hpp new file mode 100644 index 000000000..e70020f6d --- /dev/null +++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionADParams.hpp @@ -0,0 +1,168 @@ +#pragma once + +#ifdef CPPADCG + +#include +#include + +#include +#include +#include + +#include "CostFunctionQuadratic.hpp" +#include "utility/utilities.hpp" + +#include "term/TermLoadMacros.hpp" + +namespace ct { +namespace optcon { + +/** + * \ingroup CostFunction + * + * \brief Cost Function with Auto-Diff support, allowing passive parameters + * + * Custom AD cost function supporting passive parameters. Does not provide + * all features from CostFunctionAD. Demonstrates how to use dynamic parameters + * (passive w.r.t. differentiation, "non-independent variables") which can be updated + * between optimization calls using the setCurrentParameters function. + * Useful for incorporating e.g. environment data in the cost function. + * + * This is rather an ugly hack as it integrates the passive parameters in the + * state vector. This unnecessarily increases the JIT compile-time, since discrete + * environment data usually consists of hundreds of samples. + * + * A way cleaner and better way would be to use CppAD's dynamic parameters once + * this feature is available for CppADCodeGen. + * See https://github.com/joaoleal/CppADCodeGen/issues/18 + * + */ +template +class CostFunctionADParams : public CostFunctionQuadratic +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef core::DerivativesCppadJIT JacCG; + typedef typename JacCG::CG_SCALAR CGScalar; + typedef Eigen::Matrix MatrixCg; + + typedef Eigen::Matrix state_matrix_t; + typedef Eigen::Matrix control_matrix_t; + typedef Eigen::Matrix control_state_matrix_t; + + typedef core::StateVector state_vector_t; + typedef Eigen::Matrix params_vector_t; + typedef core::ControlVector control_vector_t; + typedef Eigen::Matrix VectorXs; + typedef Eigen::Matrix MatrixXs; + + /** + * \brief Basic constructor + */ + CostFunctionADParams(); + + /** + * Deep-cloning of cost function + * @return base pointer to clone + */ + CostFunctionADParams* clone() const override; + + /** + * \brief Copy constructor + * @param arg cost function to copy + */ + CostFunctionADParams(const CostFunctionADParams& arg); + + + /** + * \brief Destructor + */ + virtual ~CostFunctionADParams(); + + /** + * @brief Initializes the AD costfunction, generates and compiles + * source code + */ + virtual void initialize() override; + + /** + * \brief Add an intermediate, auto-differentiable term + * + * Use this function to add an auto-differentiable, intermediate term to the cost function. + * + * @param term The term to be added + * @param verbose Flag enabling printouts + * @return + */ + void addIntermediateADTerm(std::shared_ptr> term, + bool verbose = false) override; + + void addIntermediateADTermParam(std::shared_ptr> term, + bool verbose = false); + + /** + * \brief Add a final, auto-differentiable term + * + * Use this function to add an auto-differentiable, final term to the cost function. + * + * @param term The term to be added + * @param verbose Flag enabling printouts + * @return + */ + void addFinalADTerm(std::shared_ptr> term, + bool verbose = false) override; + + void addFinalADTermParam(std::shared_ptr> term, + bool verbose = false); + + void setCurrentStateAndControl(const state_vector_t& x, const control_vector_t& u, const SCALAR& t = 0.0) override; + + void setCurrentParameters(const params_vector_t& p); + + SCALAR evaluateIntermediate() override; + SCALAR evaluateTerminal() override; + + state_vector_t stateDerivativeIntermediate() override; + state_vector_t stateDerivativeTerminal() override; + + control_vector_t controlDerivativeIntermediate() override; + control_vector_t controlDerivativeTerminal() override; + + state_matrix_t stateSecondDerivativeIntermediate() override; + state_matrix_t stateSecondDerivativeTerminal() override; + + control_matrix_t controlSecondDerivativeIntermediate() override; + control_matrix_t controlSecondDerivativeTerminal() override; + + control_state_matrix_t stateControlDerivativeIntermediate() override; + control_state_matrix_t stateControlDerivativeTerminal() override; + + +private: + MatrixCg evaluateIntermediateCg(const Eigen::Matrix& stateInputTime); + MatrixCg evaluateTerminalCg(const Eigen::Matrix& stateInputTime); + + //! combined state, control, time and parameter vector + Eigen::Matrix stateParamsControlTime_; + + //! intermediate AD terms + std::vector>> intermediateTerms_; + std::vector>> intermediateTermsParam_; + //! final AD terms + std::vector>> finalTerms_; + std::vector>> finalTermsParam_; + + //! generated jacobians + std::shared_ptr intermediateCostCodegen_; + std::shared_ptr finalCostCodegen_; + + //! cppad functions + typename JacCG::FUN_TYPE_CG intermediateFun_; + typename JacCG::FUN_TYPE_CG finalFun_; +}; + +} // namespace optcon +} // namespace ct + +#endif