Crocoddyl
control.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, LAAS-CNRS, New York University, Max Planck
5 // Gesellschaft,
6 // University of Edinburgh, University of Trento,
7 // Heriot-Watt University
8 // Copyright note valid unless otherwise stated in individual files.
9 // All rights reserved.
11 
12 #ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
13 #define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
14 
15 #include <boost/make_shared.hpp>
16 #include <boost/shared_ptr.hpp>
17 #include <vector>
18 
19 #include "crocoddyl/core/control-base.hpp"
20 #include "crocoddyl/core/fwd.hpp"
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
26  : public ControlParametrizationModelAbstractTpl<_Scalar> {
27  public:
28  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 
30  typedef _Scalar Scalar;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38 
44  explicit ControlParametrizationModelNumDiffTpl(boost::shared_ptr<Base> model);
46 
54  void calc(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
55  const Scalar t, const Eigen::Ref<const VectorXs>& u) const;
56 
65  void calcDiff(
66  const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
67  const Scalar t, const Eigen::Ref<const VectorXs>& u) const;
68 
74  virtual boost::shared_ptr<ControlParametrizationDataAbstract> createData();
75 
84  void params(const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
85  const Scalar t, const Eigen::Ref<const VectorXs>& w) const;
86 
96  void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
97  const Eigen::Ref<const VectorXs>& w_ub,
98  Eigen::Ref<VectorXs> u_lb,
99  Eigen::Ref<VectorXs> u_ub) const;
100 
113  const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
114  const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
115  const AssignmentOp = setto) const;
116 
129  const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
130  const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
131  const AssignmentOp = setto) const;
132 
138  const boost::shared_ptr<Base>& get_model() const;
139 
144  const Scalar get_disturbance() const;
145 
150  void set_disturbance(const Scalar disturbance);
151 
152  private:
153  boost::shared_ptr<Base>
154  model_;
155  Scalar e_jac_;
157 
158  protected:
159  using Base::nu_;
160  using Base::nw_;
161 };
162 
163 template <typename _Scalar>
165  : public ControlParametrizationDataAbstractTpl<_Scalar> {
166  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 
168  typedef _Scalar Scalar;
170  typedef typename MathBase::VectorXs VectorXs;
171  typedef typename MathBase::MatrixXs MatrixXs;
173 
174  template <template <typename Scalar> class Model>
175  explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model)
176  : Base(model), du(model->get_nu()) {
177  du.setZero();
178 
179  const std::size_t nu = model->get_model()->get_nu();
180  data_0 = model->get_model()->createData();
181  for (std::size_t i = 0; i < nu; ++i) {
182  data_u.push_back(model->get_model()->createData());
183  }
184  }
185 
187 
188  VectorXs du;
189  boost::shared_ptr<Base> data_0;
190  std::vector<boost::shared_ptr<Base> >
192 };
193 
194 } // namespace crocoddyl
195 
196 /* --- Details -------------------------------------------------------------- */
197 /* --- Details -------------------------------------------------------------- */
198 /* --- Details -------------------------------------------------------------- */
199 #include "crocoddyl/core/numdiff/control.hxx"
200 
201 #endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
Abstract class for the control trajectory parametrization.
std::size_t nu_
Control parameters dimension.
const Scalar get_disturbance() const
Return the disturbance constant used in the numerical differentiation routine.
ControlParametrizationModelNumDiffTpl(boost::shared_ptr< Base > model)
Construct a new ControlParametrizationModelNumDiff object.
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.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used in the numerical differentiation routine.
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.
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.
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...
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
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...
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...
virtual boost::shared_ptr< ControlParametrizationDataAbstract > createData()
Create the control-parametrization data.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
Definition: control.hpp:191
boost::shared_ptr< Base > data_0
The data that contains the final results.
Definition: control.hpp:189
VectorXs du
temporary variable used for finite differencing
Definition: control.hpp:188