abstract_ddp_solver.h

Go to the documentation of this file.

//
// Copyright (c) 2019-2020, University of Edinburgh, University of Oxford
// 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  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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 EXOTICA_DDP_SOLVER_ABSTRACT_DDP_SOLVER_H_
#define EXOTICA_DDP_SOLVER_ABSTRACT_DDP_SOLVER_H_

#include <exotica_core/feedback_motion_solver.h>
#include <exotica_core/problems/dynamic_time_indexed_shooting_problem.h>
#include <exotica_core/server.h>
#include <exotica_core/tools/conversions.h>
#include <exotica_ddp_solver/abstract_ddp_solver_initializer.h>

namespace exotica
{
// \brief Base DDP Solver class that implements the forward pass.
//  and utility functions.
class AbstractDDPSolver : public FeedbackMotionSolver
{
public:
    void Solve(Eigen::MatrixXd& solution) override;

    void SpecifyProblem(PlanningProblemPtr pointer) override;

    Eigen::VectorXd GetFeedbackControl(Eigen::VectorXdRefConst x, int t) const override;

    const std::vector<Eigen::MatrixXd>& get_Vxx() const;
    const std::vector<Eigen::VectorXd>& get_Vx() const;
    const std::vector<Eigen::MatrixXd>& get_Qxx() const;
    const std::vector<Eigen::MatrixXd>& get_Qux() const;
    const std::vector<Eigen::MatrixXd>& get_Quu() const;
    const std::vector<Eigen::VectorXd>& get_Qx() const;
    const std::vector<Eigen::VectorXd>& get_Qu() const;
    const std::vector<Eigen::MatrixXd>& get_K() const;
    const std::vector<Eigen::VectorXd>& get_k() const;

    const std::vector<Eigen::VectorXd>& get_X_try() const;
    const std::vector<Eigen::VectorXd>& get_U_try() const;

    const std::vector<Eigen::VectorXd>& get_X_ref() const;
    const std::vector<Eigen::VectorXd>& get_U_ref() const;

    const std::vector<Eigen::MatrixXd>& get_Quu_inv() const;
    const std::vector<Eigen::MatrixXd>& get_fx() const;
    const std::vector<Eigen::MatrixXd>& get_fu() const;

    std::vector<double> get_control_cost_evolution() const;
    void set_control_cost_evolution(const int index, const double cost);

    std::vector<double> get_steplength_evolution() const;
    // void set_steplength_evolution(const int index, const double cost);

    std::vector<double> get_regularization_evolution() const;
    // void set_regularization_evolution(const int index, const double cost);

protected:
    DynamicTimeIndexedShootingProblemPtr prob_;  
    DynamicsSolverPtr dynamics_solver_;          

    virtual void BackwardPass() = 0;

    double ForwardPass(const double alpha);

    AbstractDDPSolverInitializer base_parameters_;

    virtual void IncreaseRegularization()
    {
        lambda_ *= 10.;
    }

    virtual void DecreaseRegularization()
    {
        lambda_ /= 10.;
    }

    // Local variables used in the solver - copies get updated at the beginning of solve:
    Eigen::VectorXd alpha_space_;  
    double lambda_;                
    int T_;                        
    int NU_;                       
    int NX_;                       
    int NDX_;                      
    int NV_;                       
    double dt_;                    
    double cost_;                  
    double control_cost_;          

    double cost_try_;          
    double control_cost_try_;  

    double cost_prev_;   
    double alpha_best_;  
    double time_taken_forward_pass_, time_taken_backward_pass_;

    std::vector<Eigen::MatrixXd> Vxx_;  
    std::vector<Eigen::VectorXd> Vx_;   
    std::vector<Eigen::MatrixXd> Qxx_;  
    std::vector<Eigen::MatrixXd> Qux_;  
    std::vector<Eigen::MatrixXd> Quu_;  
    std::vector<Eigen::VectorXd> Qx_;   
    std::vector<Eigen::VectorXd> Qu_;   
    std::vector<Eigen::MatrixXd> K_;    
    std::vector<Eigen::VectorXd> k_;    

    std::vector<Eigen::VectorXd> X_try_;  
    std::vector<Eigen::VectorXd> U_try_;  

    std::vector<Eigen::VectorXd> X_ref_;  
    std::vector<Eigen::VectorXd> U_ref_;  

    std::vector<Eigen::MatrixXd> Quu_inv_;  
    std::vector<Eigen::MatrixXd> fx_;       
    std::vector<Eigen::MatrixXd> fu_;       

    std::vector<double> control_cost_evolution_;    
    std::vector<double> steplength_evolution_;      
    std::vector<double> regularization_evolution_;  
};

}  // namespace exotica

#endif  // EXOTICA_DDP_SOLVER_ABSTRACT_DDP_SOLVER_H_