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"
41 template <
typename _Scalar>
42 class ActionModelNumDiffTpl :
public ActionModelAbstractTpl<_Scalar> {
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 typedef _Scalar Scalar;
47 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
48 typedef ActionModelAbstractTpl<Scalar> Base;
49 typedef ActionDataNumDiffTpl<Scalar> Data;
50 typedef MathBaseTpl<Scalar> MathBase;
51 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
52 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
66 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
67 const Eigen::Ref<const VectorXs>& u);
72 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
77 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
78 const Eigen::Ref<const VectorXs>& u);
84 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
89 virtual boost::shared_ptr<ActionDataAbstract>
createData();
94 const boost::shared_ptr<Base>&
get_model()
const;
132 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
134 boost::shared_ptr<Base> model_;
136 bool with_gauss_approx_;
139 template <
typename _Scalar>
140 struct ActionDataNumDiffTpl :
public ActionDataAbstractTpl<_Scalar> {
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 typedef _Scalar Scalar;
144 typedef MathBaseTpl<Scalar> MathBase;
145 typedef ActionDataAbstractTpl<Scalar> Base;
146 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
147 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
155 template <
template <
typename Scalar>
class Model>
158 Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
159 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
160 dx(model->get_model()->get_state()->get_ndx()),
161 du(model->get_model()->get_nu()),
162 xp(model->get_model()->get_state()->get_nx()) {
169 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
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 < ndx; ++i) {
173 data_x.push_back(model->get_model()->createData());
175 for (std::size_t i = 0; i < nu; ++i) {
176 data_u.push_back(model->get_model()->createData());
197 std::vector<boost::shared_ptr<Base> >
data_x;
198 std::vector<boost::shared_ptr<Base> >
data_u;
206 #include "crocoddyl/core/numdiff/action.hxx"
208 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_