OptVectorDms.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 <Eigen/Core>
#include <map>

#include "TimeGrid.h"

#include <ct/optcon/dms/dms_core/DmsDimensions.h>
#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
#include <ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h>
#include <ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h>
#include <ct/optcon/nlp/OptVector.h>
#include <ct/optcon/dms/dms_core/DmsSettings.h>


namespace ct {
namespace optcon {


template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class OptVectorDms : public tpl::OptVector<SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
    typedef tpl::OptVector<SCALAR> Base;

    typedef typename DIMENSIONS::state_vector_t state_vector_t;
    typedef typename DIMENSIONS::control_vector_t control_vector_t;
    typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
    typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
    typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
    typedef typename DIMENSIONS::time_array_t time_array_t;

    OptVectorDms(size_t n, const DmsSettings& settings);

    OptVectorDms(const OptVectorDms& arg) = delete;

    ~OptVectorDms() override = default;

    state_vector_t getOptimizedState(const size_t pairNum) const;

    control_vector_t getOptimizedControl(const size_t pairNum) const;

    const state_vector_array_t& getOptimizedStates();

    const control_vector_array_t& getOptimizedInputs();

    size_t getStateIndex(const size_t pairNum) const;

    size_t getControlIndex(const size_t pairNum) const;

    void setInitGuess(const state_vector_t& x0, const state_vector_t& x_f, const control_vector_t& u0);

    void setInitGuess(const state_vector_array_t& x_init, const control_vector_array_t& u_init);

    void changeInitialState(const state_vector_t& x0);

    void changeDesiredState(const state_vector_t& xF);

    size_t numPairs() { return numPairs_; }
    void printoutSolution();

private:
    DmsSettings settings_;

    const size_t numPairs_;
    /* maps the number of a "pair" to the index in w where ... */
    std::map<size_t, size_t> pairNumToStateIdx_;   /* ... its state starts */
    std::map<size_t, size_t> pairNumToControlIdx_; /* ... its control starts */

    state_vector_array_t stateSolution_;
    control_vector_array_t inputSolution_;
};

}  // namespace optcon
}  // namespace ct

#include "implementation/OptVectorDms-impl.h"