10 #ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/action-base.hpp"
51 template <
typename _Scalar>
52 class ActionModelNumDiffTpl :
public ActionModelAbstractTpl<_Scalar> {
54 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56 typedef _Scalar Scalar;
57 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
58 typedef ActionModelAbstractTpl<Scalar> Base;
59 typedef ActionDataNumDiffTpl<Scalar> Data;
60 typedef MathBaseTpl<Scalar> MathBase;
61 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
62 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
79 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u);
85 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
86 const Eigen::Ref<const VectorXs>& u);
93 virtual boost::shared_ptr<ActionDataAbstract>
createData();
100 const boost::shared_ptr<Base>&
get_model()
const;
145 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
151 boost::shared_ptr<Base> model_;
160 template <
typename _Scalar>
161 struct ActionDataNumDiffTpl :
public ActionDataAbstractTpl<_Scalar> {
162 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164 typedef _Scalar Scalar;
165 typedef MathBaseTpl<Scalar> MathBase;
166 typedef ActionDataAbstractTpl<Scalar> Base;
167 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
168 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
176 template <
template <
typename Scalar>
class Model>
179 Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
180 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
181 dx(model->get_model()->get_state()->get_ndx()),
182 du(model->get_model()->get_nu()),
183 xp(model->get_model()->get_state()->get_nx()) {
190 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
191 const std::size_t& nu = model->get_model()->get_nu();
192 data_0 = model->get_model()->createData();
193 for (std::size_t i = 0; i < ndx; ++i) {
194 data_x.push_back(model->get_model()->createData());
196 for (std::size_t i = 0; i < nu; ++i) {
197 data_u.push_back(model->get_model()->createData());
218 std::vector<boost::shared_ptr<Base> >
data_x;
219 std::vector<boost::shared_ptr<Base> >
data_u;
227 #include "crocoddyl/core/numdiff/action.hxx"
229 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_