11 #ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
12 #define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
17 #include "crocoddyl/core/diff-action-base.hpp"
46 template <
typename _Scalar>
47 class DifferentialActionModelNumDiffTpl
48 :
public DifferentialActionModelAbstractTpl<_Scalar> {
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 typedef _Scalar Scalar;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
55 typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
56 typedef DifferentialActionDataAbstractTpl<Scalar>
57 DifferentialActionDataAbstract;
58 typedef typename MathBase::VectorXs VectorXs;
59 typedef typename MathBase::MatrixXs MatrixXs;
70 boost::shared_ptr<Base> model,
const bool with_gauss_approx =
false);
77 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
78 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
86 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x);
93 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
94 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
102 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
103 const Eigen::Ref<const VectorXs>& x);
108 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
114 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
115 Eigen::Ref<VectorXs> u,
const Eigen::Ref<const VectorXs>& x,
116 const std::size_t maxiter = 100,
const Scalar tol = Scalar(1e-9));
122 const boost::shared_ptr<Base>&
get_model()
const;
146 virtual void print(std::ostream& os)
const;
159 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
160 boost::shared_ptr<Base> model_;
161 bool with_gauss_approx_;
168 template <
typename _Scalar>
169 struct DifferentialActionDataNumDiffTpl
170 :
public DifferentialActionDataAbstractTpl<_Scalar> {
171 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
173 typedef _Scalar Scalar;
174 typedef MathBaseTpl<Scalar> MathBase;
175 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
176 typedef typename MathBase::VectorXs VectorXs;
177 typedef typename MathBase::MatrixXs MatrixXs;
185 template <
template <
typename Scalar>
class Model>
188 Rx(model->get_model()->get_nr(),
189 model->get_model()->get_state()->get_ndx()),
190 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
191 dx(model->get_model()->get_state()->get_ndx()),
192 du(model->get_model()->get_nu()),
193 xp(model->get_model()->get_state()->get_nx()) {
200 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
201 const std::size_t nu = model->get_model()->get_nu();
202 data_0 = model->get_model()->createData();
203 for (std::size_t i = 0; i < ndx; ++i) {
204 data_x.push_back(model->get_model()->createData());
206 for (std::size_t i = 0; i < nu; ++i) {
207 data_u.push_back(model->get_model()->createData());
222 Scalar xuh_hess_pow2;
228 boost::shared_ptr<Base> data_0;
229 std::vector<boost::shared_ptr<Base> > data_x;
230 std::vector<boost::shared_ptr<Base> > data_u;
255 #include "crocoddyl/core/numdiff/diff-action.hxx"
257 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_