10 #ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
15 #include "crocoddyl/core/action-base.hpp"
16 #include "crocoddyl/core/fwd.hpp"
44 template <
typename _Scalar>
45 class ActionModelNumDiffTpl :
public ActionModelAbstractTpl<_Scalar> {
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 typedef _Scalar Scalar;
50 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
51 typedef ActionModelAbstractTpl<Scalar> Base;
52 typedef ActionDataNumDiffTpl<Scalar> Data;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
55 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
66 bool with_gauss_approx =
false);
72 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>& x,
74 const Eigen::Ref<const VectorXs>& u);
80 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x);
86 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& u);
94 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x);
100 virtual boost::shared_ptr<ActionDataAbstract>
createData();
105 virtual void quasiStatic(
const boost::shared_ptr<ActionDataAbstract>& data,
106 Eigen::Ref<VectorXs> u,
107 const Eigen::Ref<const VectorXs>& x,
108 const std::size_t maxiter = 100,
109 const Scalar tol = Scalar(1e-9));
114 const boost::shared_ptr<Base>&
get_model()
const;
138 virtual void print(std::ostream& os)
const;
162 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
164 boost::shared_ptr<Base> model_;
169 bool with_gauss_approx_;
174 template <
typename _Scalar>
175 struct ActionDataNumDiffTpl :
public ActionDataAbstractTpl<_Scalar> {
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
178 typedef _Scalar Scalar;
179 typedef MathBaseTpl<Scalar> MathBase;
180 typedef ActionDataAbstractTpl<Scalar> Base;
181 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
182 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
190 template <
template <
typename Scalar>
class Model>
193 Rx(model->get_model()->get_nr(),
194 model->get_model()->get_state()->get_ndx()),
195 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
196 dx(model->get_model()->get_state()->get_ndx()),
197 du(model->get_model()->get_nu()),
198 xp(model->get_model()->get_state()->get_nx()) {
205 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
206 const std::size_t nu = model->get_model()->get_nu();
207 data_0 = model->get_model()->createData();
208 for (std::size_t i = 0; i < ndx; ++i) {
209 data_x.push_back(model->get_model()->createData());
211 for (std::size_t i = 0; i < nu; ++i) {
212 data_u.push_back(model->get_model()->createData());
238 Scalar xuh_hess_pow2;
244 boost::shared_ptr<Base>
data_0;
246 std::vector<boost::shared_ptr<Base> >
248 std::vector<boost::shared_ptr<Base> >
257 #include "crocoddyl/core/numdiff/action.hxx"
259 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_