crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
control.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, New York University, Max Planck Gesellschaft,
5 // University of Edinburgh, University of Trento
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
12 
13 #include <boost/make_shared.hpp>
14 #include <boost/shared_ptr.hpp>
15 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/control-base.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
22 class ControlParametrizationModelNumDiffTpl : public ControlParametrizationModelAbstractTpl<_Scalar> {
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
27  typedef MathBaseTpl<Scalar> MathBase;
28  typedef ControlParametrizationModelAbstractTpl<_Scalar> Base;
29  typedef ControlParametrizationDataNumDiffTpl<_Scalar> Data;
30  typedef ControlParametrizationDataAbstractTpl<_Scalar> ControlParametrizationDataAbstract;
31  typedef typename MathBase::VectorXs VectorXs;
32  typedef typename MathBase::MatrixXs MatrixXs;
33 
39  explicit ControlParametrizationModelNumDiffTpl(boost::shared_ptr<Base> model);
41 
49  void calc(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
50  const Eigen::Ref<const VectorXs>& u) const;
51 
59  void calcDiff(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
60  const Eigen::Ref<const VectorXs>& u) const;
61 
67  virtual boost::shared_ptr<ControlParametrizationDataAbstract> createData();
68 
77  void params(const boost::shared_ptr<ControlParametrizationDataAbstract>& data, const Scalar t,
78  const Eigen::Ref<const VectorXs>& w) const;
79 
88  void convertBounds(const Eigen::Ref<const VectorXs>& w_lb, const Eigen::Ref<const VectorXs>& w_ub,
89  Eigen::Ref<VectorXs> u_lb, Eigen::Ref<VectorXs> u_ub) const;
90 
100  void multiplyByJacobian(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
101  const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
102  const AssignmentOp = setto) const;
103 
114  void multiplyJacobianTransposeBy(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
115  const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
116  const AssignmentOp = setto) const;
117 
123  const boost::shared_ptr<Base>& get_model() const;
124 
130  const Scalar get_disturbance() const;
131 
137  void set_disturbance(const Scalar disturbance);
138 
139  private:
144  boost::shared_ptr<Base> model_;
145 
149  Scalar disturbance_;
150 
151  protected:
152  using Base::nu_;
153  using Base::nw_;
154 };
155 
156 template <typename _Scalar>
157 struct ControlParametrizationDataNumDiffTpl : public ControlParametrizationDataAbstractTpl<_Scalar> {
158  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159 
160  typedef _Scalar Scalar;
161  typedef MathBaseTpl<Scalar> MathBase;
162  typedef typename MathBase::VectorXs VectorXs;
163  typedef typename MathBase::MatrixXs MatrixXs;
164  typedef ControlParametrizationDataAbstractTpl<Scalar> Base;
165 
166  template <template <typename Scalar> class Model>
167  explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model) : Base(model), du(model->get_nu()) {
168  du.setZero();
169 
170  const std::size_t nu = model->get_model()->get_nu();
171  data_0 = model->get_model()->createData();
172  for (std::size_t i = 0; i < nu; ++i) {
173  data_u.push_back(model->get_model()->createData());
174  }
175  }
176 
177  virtual ~ControlParametrizationDataNumDiffTpl() {}
178 
179  VectorXs du;
180  boost::shared_ptr<Base> data_0;
181  std::vector<boost::shared_ptr<Base> > data_u;
182 };
183 
184 } // namespace crocoddyl
185 
186 /* --- Details -------------------------------------------------------------- */
187 /* --- Details -------------------------------------------------------------- */
188 /* --- Details -------------------------------------------------------------- */
189 #include "crocoddyl/core/numdiff/control.hxx"
190 
191 #endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
crocoddyl::ControlParametrizationModelNumDiffTpl::createData
virtual boost::shared_ptr< ControlParametrizationDataAbstract > createData()
Create the control-parametrization data.
crocoddyl::ControlParametrizationModelNumDiffTpl::calcDiff
void calcDiff(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const
Get the value of the Jacobian of the control with respect to the parameters.
crocoddyl::ControlParametrizationModelNumDiffTpl::get_model
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
crocoddyl::ControlParametrizationModelNumDiffTpl::multiplyJacobianTransposeBy
void multiplyJacobianTransposeBy(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const
Compute the product between the transposed Jacobian of the control (with respect to the parameters) a...
crocoddyl::ControlParametrizationDataNumDiffTpl::data_0
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: control.hpp:180
crocoddyl::ControlParametrizationModelNumDiffTpl::multiplyByJacobian
void multiplyByJacobian(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Eigen::Ref< const MatrixXs > &A, Eigen::Ref< MatrixXs > out, const AssignmentOp=setto) const
Compute the product between a specified matrix and the Jacobian of the control (with respect to the p...
crocoddyl::ControlParametrizationModelAbstractTpl::nu_
std::size_t nu_
Control parameters dimension.
Definition: control-base.hpp:164
crocoddyl::ControlParametrizationModelNumDiffTpl::get_disturbance
const Scalar get_disturbance() const
Get the disturbance_ object.
crocoddyl::ControlParametrizationModelNumDiffTpl::convertBounds
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
Convert the bounds on the control to bounds on the control parameters.
crocoddyl::ControlParametrizationDataNumDiffTpl::data_u
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: control.hpp:181
crocoddyl::ControlParametrizationDataNumDiffTpl::du
VectorXs du
temporary variable used for finite differencing
Definition: control.hpp:179
crocoddyl::ControlParametrizationModelAbstractTpl::nw_
std::size_t nw_
Control dimension.
Definition: control-base.hpp:163
crocoddyl::ControlParametrizationModelNumDiffTpl::calc
void calc(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &u) const
Get the value of the control at the specified time.
crocoddyl::ControlParametrizationModelNumDiffTpl::params
void params(const boost::shared_ptr< ControlParametrizationDataAbstract > &data, const Scalar t, const Eigen::Ref< const VectorXs > &w) const
Get a value of the control parameters such that the control at the specified time t is equal to the s...
crocoddyl::ControlParametrizationModelNumDiffTpl::ControlParametrizationModelNumDiffTpl
ControlParametrizationModelNumDiffTpl(boost::shared_ptr< Base > model)
Construct a new ControlParametrizationModelNumDiff object.
crocoddyl::ControlParametrizationModelNumDiffTpl::set_disturbance
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.