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>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
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;
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;
144 boost::shared_ptr<Base> model_;
156 template <
typename _Scalar>
158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
160 typedef _Scalar Scalar;
162 typedef typename MathBase::VectorXs VectorXs;
163 typedef typename MathBase::MatrixXs MatrixXs;
166 template <
template <
typename Scalar>
class Model>
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());
181 std::vector<boost::shared_ptr<Base> >
data_u;
189 #include "crocoddyl/core/numdiff/control.hxx"
Abstract class for the control trajectory parametrization.
std::size_t nw_
Control dimension.
std::size_t nu_
Control parameters dimension.
const Scalar get_disturbance() const
Get the disturbance_ object.
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)
Set the disturbance_ object.
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.
boost::shared_ptr< Base > data_0
The data that contains the final results.
VectorXs du
temporary variable used for finite differencing