10 #ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
13 #include <boost/make_shared.hpp>
14 #include <boost/shared_ptr.hpp>
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/control-base.hpp"
21 template <
typename _Scalar>
22 class ControlParametrizationModelNumDiffTpl :
public ControlParametrizationModelAbstractTpl<_Scalar> {
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
49 void calc(
const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
const Scalar t,
50 const Eigen::Ref<const VectorXs>& u)
const;
59 void calcDiff(
const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
const Scalar t,
60 const Eigen::Ref<const VectorXs>& u)
const;
67 virtual boost::shared_ptr<ControlParametrizationDataAbstract>
createData();
77 void params(
const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
const Scalar t,
78 const Eigen::Ref<const VectorXs>& w)
const;
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;
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;
115 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
116 const AssignmentOp = setto)
const;
123 const boost::shared_ptr<Base>&
get_model()
const;
144 boost::shared_ptr<Base> model_;
156 template <
typename _Scalar>
157 struct ControlParametrizationDataNumDiffTpl :
public ControlParametrizationDataAbstractTpl<_Scalar> {
158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
166 template <
template <
typename Scalar>
class Model>
167 explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>*
const model) : Base(model),
du(model->get_nu()) {
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());
177 virtual ~ControlParametrizationDataNumDiffTpl() {}
181 std::vector<boost::shared_ptr<Base> >
data_u;
189 #include "crocoddyl/core/numdiff/control.hxx"
191 #endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_