poly-two-rk.hpp

// BSD 3-Clause License
//
// Copyright (C) 2021, University of Edinburgh, University of Trento
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
#define CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_
 
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/control-base.hpp"
#include "crocoddyl/core/integrator/rk.hpp"
#include "crocoddyl/core/utils/exception.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class ControlParametrizationModelPolyTwoRKTpl : public ControlParametrizationModelAbstractTpl<_Scalar> {
 public:
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
  typedef ControlParametrizationModelAbstractTpl<Scalar> Base;
  typedef ControlParametrizationDataPolyTwoRKTpl<Scalar> Data;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  explicit ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw, const RKType rktype);
  virtual ~ControlParametrizationModelPolyTwoRKTpl();
 
  virtual void calc(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
                    const Eigen::Ref<const VectorXs>& u) const;
 
  virtual void calcDiff(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
                        const Eigen::Ref<const VectorXs>& u) const;
 
  virtual boost::shared_ptr<ControlParametrizationDataAbstract> createData();
 
  virtual void params(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
                      const Eigen::Ref<const VectorXs>& w) const;
 
  virtual void convertBounds(const Eigen::Ref<const VectorXs>& w_lb, const Eigen::Ref<const VectorXs>& w_ub,
                             Eigen::Ref<VectorXs> u_lb, Eigen::Ref<VectorXs> u_ub) const;
 
  virtual void multiplyByJacobian(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
                                  const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
                                  const AssignmentOp = setto) const;
 
  virtual void multiplyJacobianTransposeBy(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
                                           const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
                                           const AssignmentOp = setto) const;
 
 protected:
  using Base::nu_;
  using Base::nw_;
 
 private:
  RKType rktype_;
};
 
template <typename _Scalar>
struct ControlParametrizationDataPolyTwoRKTpl : public ControlParametrizationDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ControlParametrizationDataAbstractTpl<Scalar> Base;
  typedef typename MathBase::Vector3s Vector3s;
 
  template <template <typename Scalar> class Model>
  explicit ControlParametrizationDataPolyTwoRKTpl(Model<Scalar>* const model) : Base(model), tmp_t2(0.) {
    c.setZero();
  }
 
  virtual ~ControlParametrizationDataPolyTwoRKTpl() {}
 
  Vector3s c;     
  Scalar tmp_t2;  
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/controls/poly-two-rk.hxx"
 
#endif  // CROCODDYL_CORE_CONTROLS_POLY_TWO_RK_HPP_