base_method_interface.hpp

Go to the documentation of this file.

/***********************************************************************************
Copyright (c) 2017, Diego Pardo. All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.
    * Neither the name of ETH ZURICH nor the names of its contributors may be used
      to endorse or promote products derived from this software without specific
      prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************/

#ifndef INCLUDE_METHOD_INTERFACES_BASE_METHOD_INTERFACE_HPP_
#define INCLUDE_METHOD_INTERFACES_BASE_METHOD_INTERFACE_HPP_

#define infy 1e20

#include <unistd.h>
#include <Eigen/Core>
#include <cmath>
#include <map>
#include <memory>
#include <vector>

#include <method_interfaces/nonlinear_program/nonlinear_program.hpp>

#include <dynamical_systems/base/DynamicsBase.hpp>
#include <dynamical_systems/base/DerivativesBaseDS.hpp>
#include <optimization/costs/CostFunctionBase.hpp>
#include <optimization/constraints/ConstraintsBase.hpp>
#include <optimization/constraints/InterPhaseBaseConstraint.hpp>
#include <optimization/constraints/GuardBase.hpp>

namespace DirectTrajectoryOptimization {

template<typename DIMENSIONS>
class DTOMethodInterface {

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    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;

    virtual ~DTOMethodInterface() {

    }

    DTOMethodInterface(
            std::vector<std::shared_ptr<DynamicsBase<DIMENSIONS> > > dynamics,
            std::vector<std::shared_ptr<DerivativesBaseDS<DIMENSIONS> > > derivatives,
            std::shared_ptr<BaseClass::CostFunctionBase<DIMENSIONS> > costFunction,
            std::vector<std::shared_ptr<BaseClass::ConstraintsBase<DIMENSIONS> > > constraints,
            Eigen::Vector2d duration, int number_of_nodes, bool methodVerbose);

    /* user functions */
    void Initialize(); // WHY is THE SOLVER Initializing the method?
    virtual void SetDircolMethod(const int &dcm) {}
    void SetInitialState(const state_vector_t & y0);
    void SetFinalState(const state_vector_t & yF);

    void SetInitialControl(const control_vector_t & u0);
    void SetFinalControl(const control_vector_t & uf);

    void SetFinalVelocityZero(const Eigen::VectorXi & indexes) {
         vel_state_to_zero.resize(indexes.rows());
         vel_state_to_zero = indexes;
        _finalVelocityZero = true;
    }

    void SetEvenTimeIncrements(bool flag) {
        _evenTimeIncr = flag;
    }
    void SetVerbosity(bool verbose) {
        _verbose = verbose;
    }

    void SetGuardFunction(std::vector<double (*)(state_vector_t&)> usrFct) {

        if (usrFct.size() != num_phases - 1) {
            throw std::invalid_argument("Vector of guard functions are of the wrong size.");
        }

        //Future: this must be a condition given a vector of dynamics and not user-dependent
        _guardVector = usrFct;
        _usingGuardFunction = true;
    }

    void SetInterPhaseFunctions(std::vector<std::shared_ptr<BaseClass::GuardBase<DIMENSIONS> > > gconst,
                                std::vector<std::shared_ptr<BaseClass::InterPhaseBase<DIMENSIONS>>> intph) {

        if (gconst.size() != num_phases - 1) {
            throw std::invalid_argument("Vector of guard functions are of the wrong size.");
        }

        if (intph.size() != num_phases - 1) {
            throw std::invalid_argument("Vector of interPhase functions are of the wrong size.");
        }

        _guards = gconst;
        _interphase_constraint = intph;

        //_usingGuardFunction = true;
    }

    void SetPercentageNodesPerDynamics(Eigen::VectorXd nodeSplit);
    void SetNodesPerPhase(const Eigen::VectorXi & nodes);
    void SetStateBounds(const state_vector_t & lb, const state_vector_t & ub);
    void SetStateBounds(const state_vector_array_t & lb, const state_vector_array_t & ub);
    void SetControlBounds(const control_vector_t & lb, const control_vector_t & ub);
    void SetControlBounds(const control_vector_array_t & lb, const control_vector_array_t & ub);
    void SetFreeFinalState(int state_number);

    // Check this!!, redundat w.r.t. solver?!
    void InitializeNLPvector(Eigen::VectorXd & x,
            const state_vector_array_t & state_initial,
            const control_vector_array_t & control_initial,
            const Eigen::VectorXd & h_initial);

    void setTimeIncrementsBounds(const double & h_min, const double & h_max);

    /* Public Methods allow to collect data from the solver app */
    inline int getNumDefects() {
        return num_defects;
    }

    // Check this! remove TEMPLATE from SOLVER
    inline int getNumStates() {
        return _numStates;
    }
    inline int getNumActions() {
        return _numControls;
    }
    inline int getNumIncrements() {
        return n_inc;
    }

    // The solver is using this for set names of the variables?
    // ToDo: Bring that to this class
    void getTrajectoryIndexes(Eigen::VectorXi &hs, Eigen::MatrixXi &ys,
                Eigen::MatrixXi &us);

    // CHECK: Deprecated way of getting the DTO solution
    // ToDo : User should get solution from THIS CLASS
    // ToDo : when finished, solver should update the DTO NLP member
    //  int getSolution(Eigen::VectorXd &hs, Eigen::MatrixXd &ys,
    //          Eigen::MatrixXd &us, Eigen::VectorXd & contact_switching_index);

    // Solver interface functions
    void UpdateDecisionVariables(const double* x);
    void evalObjective(double *f);
    void evalConstraintFct(double* g, const int& m);

    void evalSparseJacobianCost(double* val);
    virtual void evalSparseJacobianConstraint(double* val) = 0;

protected :

    /* Called during class initialization */
    void SetSizeOfVariables();
    void SetSizeOfConstraints();
    void SetTOConstraintsBounds();
    void SetTOVariablesBounds();
    void SetupJacobianVariables();
    virtual void Method_specific_initialization() {}; // virtual method to allow specific initialization of the methods

    /* setting G-Jacobian sparsity */
    void setNLPnnzConstraintJacobian();
    void setSparsityJacobianCost();
    void setSparsityJacobianConstraint();

    /* auxiliary for setting G-Jacobian sparsity */
    int getNumNonZeroUserJacobian();
    int getNumNonZeroGuardFunctionJacobian();
    int getNumNonZeroInterphaseJacobian();

    /* setting Gcost-Jacobian sparsity */
    void setNLPnnzCostFunctionJacobian();

    /* validate user inputs */
    bool ValidateDuration();
    bool ValidateStateControlBounds();

    /* required to getConstraintFct */
    virtual void evaluateDefects(Eigen::VectorXd & defect_vector) = 0;
    void evaluateParametricConstraints(Eigen::VectorXd & parametric_vector);
    void evaluateUserConstraints(Eigen::VectorXd & constraints_vector);
    void evaluateGuardConstraints(Eigen::VectorXd & constraints_vector);
    void evaluateInterphaseConstraints(Eigen::VectorXd & constraints_vector);


public:

    void getStateControlIncrementsTrajectoriesFromDecisionVariables(
            const Eigen::Map<Eigen::VectorXd> & x_local,
            state_vector_array_t & y_trajectory,
            control_vector_array_t & u_trajectory,
            Eigen::VectorXd & h_trajectory);
    void getStateControlIncrementsTrajectoriesFromDecisionVariables(
            const Eigen::VectorXd & x_local,
            state_vector_array_t & y_trajectory,
            control_vector_array_t & u_trajectory,
            Eigen::VectorXd & h_trajectory);

    void UpdateDTOTrajectoriesFromDecisionVariables(
            const Eigen::Map<Eigen::VectorXd> & x_local);

    void getCompleteSolution(state_vector_array_t & yTraj,
                             state_vector_array_t & ydotTraj,
                             control_vector_array_t & uTraj,
                             Eigen::VectorXd & hTraj,
                             std::vector<int> & phaseId) const;

    void SetDecisionVariablesFromStateControlIncrementsTrajectories(
            Eigen::VectorXd & x_vector,
            const state_vector_array_t & y_sol,
            const control_vector_array_t & u_sol,
            const Eigen::VectorXd & h_sol);

    void getProtectedVariables(state_vector_array_t & y_t , state_vector_array_t & yd_t,
                               control_vector_array_t & u_t, Eigen::VectorXd & h_t) {
       y_t = _y_trajectory;
      yd_t = _ydot_trajectory;
       u_t = _u_trajectory;
       h_t = _h_trajectory;
    }

protected:

    bool _verbose;
    int _numberOfNodes;
    int _minNodesPerPhase = 2;
    double min_traj_time = 0.01;
    double minimum_increment_size = 0.005;//0.0015;//0.0015; //0.01; // Default values for DT
    double maximum_increment_size = 0.1;//0.15; //0.2;
    bool _initialControlZero = false;
    bool _finalVelocityZero = false;
    bool _evenTimeIncr = false;
    bool _nodeSplitSet = false;
    bool _nodeDistribution = false;
    bool _usingGuardFunction = false;
    bool _multiphaseproblem = false;

    Eigen::Vector2d _trajTimeRange;
    Eigen::VectorXd _nodeSplitPercentage;

    const int _numStates;
    const int _numControls;

    int num_phases = 0;
public:
    int n_inc = 0;
    int num_defects = 0;
protected:
    int num_parametric_constraints = 0;
    int num_user_constraints = 0;
    int num_guards = 0;
    int num_interphase_constraints = 0;

    int nnz_user_constraints = 0;

    std::vector<int> num_user_constraints_per_node_in_phase;
    std::vector<int> num_nonzero_jacobian_user_per_node_in_phase;

    std::vector<int> size_interphase_constraint;
    std::vector<int> size_guard;

    // System states and actions initial and final conditions and bounds
    state_vector_t y_0, y_f;
    state_vector_array_t y_lb, y_ub;
    control_vector_t u_0, u_f;
    control_vector_array_t  u_lb, u_ub;

    // Velocity Index to set to zero
    Eigen::VectorXi vel_state_to_zero;

    // flags to verify user setting final_state
    // ToDo: convert to boolean arrays using 'typedef Matrix<bool, Dynamic, 1> VectorXb;'
    Eigen::VectorXi _flagInitialState;
    Eigen::VectorXi _flagFinalState;

    Eigen::VectorXi _flagInitialControl;
    Eigen::VectorXi _flagFinalControl;

    // indexes of the TO problem in the Optimization vector
    Eigen::VectorXi h_inds;
    Eigen::MatrixXi y_inds;
    Eigen::MatrixXi u_inds;

    // Number of node of the bounds between phases
    Eigen::VectorXi leftphase_nodes;
    Eigen::VectorXi rightphase_nodes;

    // Defect bounds (Just for readability)
    Eigen::VectorXd defects_ub;
    Eigen::VectorXd defects_lb;

    // Parametric bounds
    Eigen::VectorXd parametricConstraints_lb;
    Eigen::VectorXd parametricConstraints_ub;

    // User Constraints bounds
    Eigen::VectorXd userConstraints_lb;
    Eigen::VectorXd userConstraints_ub;

    // Guard Function bounds
    Eigen::VectorXd guard_lb;
    Eigen::VectorXd guard_ub;

    // Interphase Constraints bounds
    Eigen::VectorXd interphase_lb;
    Eigen::VectorXd interphase_ub;

    // Constraint vectors
  Eigen::VectorXd defect_out_;
  Eigen::VectorXd parametric_out_;
  Eigen::VectorXd user_constraints_out_;

  // Guard vectors
  Eigen::VectorXd guard_vector_;
  Eigen::VectorXd interface_vector_;


    // Solution to log using Cereal
    Eigen::VectorXd increments_solution;
    Eigen::VectorXd time_solution;
    Eigen::MatrixXd states_solution;
    Eigen::MatrixXd controls_solution;
    bool problem_is_infeasible = false;


public:
    // dynamic to node mapping
    // ToDo this should be a map from int to weak_ptr's but this
    // may cause issues with Eigen...
    std::vector<int> node_to_phase_map;
    std::vector<int> number_nodes_phase;
    std::vector<int> user_non_zero_G_phase;
    std::vector<double (*)(state_vector_t &)> _guardVector;
    std::vector<void (*)(state_vector_t &x, state_vector_t & J) > _guardVectorDerivative;

//protected:
    // dto trajectories
    state_vector_array_t   _y_trajectory;
    control_vector_array_t _u_trajectory;
    Eigen::VectorXd        _h_trajectory;
    state_vector_array_t   _ydot_trajectory;

public:
    // shared_ptr's
    std::vector<std::shared_ptr<DynamicsBase<DIMENSIONS> > > _dynamics;                 /* to system dynamics */
    std::vector<std::shared_ptr<DerivativesBaseDS<DIMENSIONS> > > _derivatives;         /* to dynamics derivatives */
    std::shared_ptr<BaseClass::CostFunctionBase<DIMENSIONS> > _costFunction;            /* to cost Function*/
    std::vector<std::shared_ptr<BaseClass::ConstraintsBase<DIMENSIONS> > > _constraints;/* to constraints*/
    std::vector<std::shared_ptr<BaseClass::InterPhaseBase<DIMENSIONS> > > _interphase_constraint; /* to interphase_constraints */
    std::vector<std::shared_ptr<BaseClass::GuardBase<DIMENSIONS> > > _guards; /* to guards constraints */

    //NLP
public:
    nonlinearprogram myNLP;

};

} //namespace DirectTrajectoryOptimization

#include <method_interfaces/bmi_implementation.hpp>

#endif /*INCLUDE_METHOD_INTERFACES_BASE_METHOD_INTERFACE_HPP_*/