9 #ifndef CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_ 10 #define CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_ 15 #include "crocoddyl/core/fwd.hpp" 16 #include "crocoddyl/core/actuation-base.hpp" 30 template <
typename _Scalar>
31 class ActuationModelNumDiffTpl :
public ActuationModelAbstractTpl<_Scalar> {
33 typedef _Scalar Scalar;
34 typedef MathBaseTpl<Scalar> MathBase;
35 typedef ActuationModelAbstractTpl<Scalar> Base;
36 typedef ActuationDataNumDiffTpl<Scalar> Data;
37 typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
38 typedef typename MathBase::VectorXs VectorXs;
39 typedef typename MathBase::MatrixXs MatrixXs;
56 virtual void calc(
const boost::shared_ptr<ActuationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
57 const Eigen::Ref<const VectorXs>& u);
63 virtual void calc(
const boost::shared_ptr<ActuationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
68 virtual void calcDiff(
const boost::shared_ptr<ActuationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
69 const Eigen::Ref<const VectorXs>& u);
75 virtual void calcDiff(
const boost::shared_ptr<ActuationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
80 virtual boost::shared_ptr<ActuationDataAbstract>
createData();
85 const boost::shared_ptr<Base>&
get_model()
const;
98 boost::shared_ptr<Base> model_;
105 template <
typename _Scalar>
106 struct ActuationDataNumDiffTpl :
public ActuationDataAbstractTpl<_Scalar> {
107 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109 typedef _Scalar Scalar;
110 typedef MathBaseTpl<Scalar> MathBase;
111 typedef typename MathBase::VectorXs VectorXs;
112 typedef ActuationDataAbstractTpl<Scalar> Base;
120 template <
template <
typename Scalar>
class Model>
129 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
130 const std::size_t nu = model->get_model()->get_nu();
131 data_0 = model->get_model()->createData();
132 for (std::size_t i = 0; i < ndx; ++i) {
133 data_x.push_back(model->get_model()->createData());
135 for (std::size_t i = 0; i < nu; ++i) {
136 data_u.push_back(model->get_model()->createData());
144 std::vector<boost::shared_ptr<Base> >
data_x;
145 std::vector<boost::shared_ptr<Base> >
data_u;
157 #include "crocoddyl/core/numdiff/actuation.hxx" 159 #endif // CROCODDYL_CORE_NUMDIFF_ACTUATION_HPP_ std::size_t nu_
Control dimension.
VectorXs dx
State disturbance.
ActuationDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff actuation data.
const boost::shared_ptr< Base > & get_model() const
Return the original actuation model.
virtual boost::shared_ptr< ActuationDataAbstract > createData()
Create the actuation data.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
std::size_t get_nu() const
Return the dimension of the control input.
virtual void calcDiff(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the actuation function.
boost::shared_ptr< Base > data_0
The data that contains the final results.
ActuationModelNumDiffTpl(boost::shared_ptr< Base > model)
Initialize the numdiff residual model.
virtual ~ActuationModelNumDiffTpl()
Destroy the numdiff actuation model.
VectorXs du
Control disturbance.
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
virtual void calc(const boost::shared_ptr< ActuationDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the actuation signal from the state point and control input .
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".