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/fwd.hpp"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/core/action-base.hpp"
#include "crocoddyl/core/utils/deprecate.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<boost::shared_ptr<ActionModelAbstract> >& running_models,
                     boost::shared_ptr<ActionModelAbstract> terminal_model);
 
  ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
                     boost::shared_ptr<ActionModelAbstract> terminal_model,
                     const std::vector<boost::shared_ptr<ActionDataAbstract> >& running_datas,
                     boost::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(boost::shared_ptr<ActionModelAbstract> model, boost::shared_ptr<ActionDataAbstract> data);
 
  void circularAppend(boost::shared_ptr<ActionModelAbstract> model);
 
  void updateNode(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
                  boost::shared_ptr<ActionDataAbstract> data);
 
  void updateModel(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model);
 
  std::size_t get_T() const;
 
  const VectorXs& get_x0() const;
 
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& get_runningModels() const;
 
  const boost::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
 
  const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas() const;
 
  const boost::shared_ptr<ActionDataAbstract>& get_terminalData() const;
 
  void set_x0(const VectorXs& x0_in);
 
  void set_runningModels(const std::vector<boost::shared_ptr<ActionModelAbstract> >& models);
 
  void set_terminalModel(boost::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_;                                                          
  boost::shared_ptr<ActionModelAbstract> terminal_model_;                
  boost::shared_ptr<ActionDataAbstract> terminal_data_;                  
  std::vector<boost::shared_ptr<ActionModelAbstract> > running_models_;  
  std::vector<boost::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_