10 #ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ 11 #define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ 16 #include "crocoddyl/core/diff-action-base.hpp" 20 template <
typename _Scalar>
21 class DifferentialActionModelNumDiffTpl :
public DifferentialActionModelAbstractTpl<_Scalar> {
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 typedef _Scalar Scalar;
26 typedef MathBaseTpl<Scalar> MathBase;
27 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
28 typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
29 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
33 explicit DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model,
bool with_gauss_approx =
false);
34 virtual ~DifferentialActionModelNumDiffTpl();
36 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
37 const Eigen::Ref<const VectorXs>& u);
38 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
39 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
40 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
42 const boost::shared_ptr<Base>& get_model()
const;
43 const Scalar& get_disturbance()
const;
44 void set_disturbance(
const Scalar& disturbance);
45 bool get_with_gauss_approx();
57 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
58 boost::shared_ptr<Base> model_;
59 bool with_gauss_approx_;
63 template <
typename _Scalar>
64 struct DifferentialActionDataNumDiffTpl :
public DifferentialActionDataAbstractTpl<_Scalar> {
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 typedef _Scalar Scalar;
68 typedef MathBaseTpl<Scalar> MathBase;
69 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
70 typedef typename MathBase::VectorXs VectorXs;
71 typedef typename MathBase::MatrixXs MatrixXs;
79 template <
template <
typename Scalar>
class Model>
82 Rx(model->get_model()->
get_nr(), model->get_model()->
get_state()->get_ndx()),
83 Ru(model->get_model()->
get_nr(), model->get_model()->
get_nu()),
84 dx(model->get_model()->
get_state()->get_ndx()),
85 du(model->get_model()->
get_nu()),
86 xp(model->get_model()->
get_state()->get_nx()) {
93 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
94 const std::size_t& nu = model->get_model()->get_nu();
95 data_0 = model->get_model()->createData();
96 for (std::size_t i = 0; i < ndx; ++i) {
97 data_x.push_back(model->get_model()->createData());
99 for (std::size_t i = 0; i < nu; ++i) {
100 data_u.push_back(model->get_model()->createData());
109 boost::shared_ptr<Base> data_0;
110 std::vector<boost::shared_ptr<Base> > data_x;
111 std::vector<boost::shared_ptr<Base> > data_u;
130 #include "crocoddyl/core/numdiff/diff-action.hxx" 132 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
const std::size_t & get_nr() const
Return the dimension of the cost-residual vector.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
const std::size_t & get_nu() const
Return the dimension of the control input.
VectorXs u_lb_
Lower control limits.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
std::size_t nu_
Control dimension.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs u_ub_
Upper control limits.
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration and cost value.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.