shooting.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of
// Oxford Copyright note valid unless otherwise stated in individual files. All
// rights reserved.
 
#ifndef CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
#define CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
 
#include <stdexcept>
#include <vector>
 
#include "crocoddyl/core/action-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/utils/deprecate.hpp"
#include "crocoddyl/core/utils/exception.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class ShootingProblemTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef ActionModelAbstractTpl<Scalar> ActionModelAbstract;
  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
 
  ShootingProblemTpl(
      const VectorXs& x0,
      const std::vector<std::shared_ptr<ActionModelAbstract> >& running_models,
      std::shared_ptr<ActionModelAbstract> terminal_model);
 
  ShootingProblemTpl(
      const VectorXs& x0,
      const std::vector<std::shared_ptr<ActionModelAbstract> >& running_models,
      std::shared_ptr<ActionModelAbstract> terminal_model,
      const std::vector<std::shared_ptr<ActionDataAbstract> >& running_datas,
      std::shared_ptr<ActionDataAbstract> terminal_data);
  ShootingProblemTpl(const ShootingProblemTpl<Scalar>& problem);
  ~ShootingProblemTpl();
 
  Scalar calc(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
 
  Scalar calcDiff(const std::vector<VectorXs>& xs,
                  const std::vector<VectorXs>& us);
 
  void rollout(const std::vector<VectorXs>& us, std::vector<VectorXs>& xs);
 
  std::vector<VectorXs> rollout_us(const std::vector<VectorXs>& us);
 
  void quasiStatic(std::vector<VectorXs>& us, const std::vector<VectorXs>& xs);
 
  std::vector<VectorXs> quasiStatic_xs(const std::vector<VectorXs>& xs);
 
  void circularAppend(std::shared_ptr<ActionModelAbstract> model,
                      std::shared_ptr<ActionDataAbstract> data);
 
  void circularAppend(std::shared_ptr<ActionModelAbstract> model);
 
  void updateNode(const std::size_t i,
                  std::shared_ptr<ActionModelAbstract> model,
                  std::shared_ptr<ActionDataAbstract> data);
 
  void updateModel(const std::size_t i,
                   std::shared_ptr<ActionModelAbstract> model);
 
  std::size_t get_T() const;
 
  const VectorXs& get_x0() const;
 
  const std::vector<std::shared_ptr<ActionModelAbstract> >& get_runningModels()
      const;
 
  const std::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
 
  const std::vector<std::shared_ptr<ActionDataAbstract> >& get_runningDatas()
      const;
 
  const std::shared_ptr<ActionDataAbstract>& get_terminalData() const;
 
  void set_x0(const VectorXs& x0_in);
 
  void set_runningModels(
      const std::vector<std::shared_ptr<ActionModelAbstract> >& models);
 
  void set_terminalModel(std::shared_ptr<ActionModelAbstract> model);
 
  void set_nthreads(const int nthreads);
 
  std::size_t get_nx() const;
 
  std::size_t get_ndx() const;
 
  DEPRECATED("Compute yourself the maximum dimension of the control vector",
             std::size_t get_nu_max() const;)
 
  std::size_t get_nthreads() const;
 
  bool is_updated();
 
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os,
                                  const ShootingProblemTpl<Scalar>& problem);
 
 protected:
  Scalar cost_;    
  std::size_t T_;  
  VectorXs x0_;    
  std::shared_ptr<ActionModelAbstract>
      terminal_model_;  
  std::shared_ptr<ActionDataAbstract> terminal_data_;  
  std::vector<std::shared_ptr<ActionModelAbstract> >
      running_models_;  
  std::vector<std::shared_ptr<ActionDataAbstract> >
      running_datas_;     
  std::size_t nx_;        
  std::size_t ndx_;       
  std::size_t nu_max_;    
  std::size_t nthreads_;  
  bool is_updated_;
 
 private:
  void allocateData();
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/optctrl/shooting.hxx"
 
#endif  // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_