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>
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_ virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
bool has_control_limits_
Indicates whether any of the control limits is finite.
MatrixXs Ru
Cost residual jacobian: .
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
VectorXs dx
State disturbance.
const boost::shared_ptr< Base > & get_model() const
Return the acton model that we use to numerical differentiate.
std::size_t nu_
Control dimension.
std::size_t get_nu() const
Return the dimension of the control input.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
ActionDataNumDiffTpl(Model< Scalar > *const model)
Initialize the numdiff action data.
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
boost::shared_ptr< Base > data_0
The data that contains the final results.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
ActionModelNumDiffTpl(boost::shared_ptr< Base > model, bool with_gauss_approx=false)
Initialize the numdiff action model.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
MatrixXs Rx
Cost residual jacobian: .
VectorXs unone_
Neutral state.
VectorXs u_ub_
Upper control limits.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
VectorXs u_lb_
Lower control limits.
boost::shared_ptr< StateAbstract > state_
Model of the state.
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
VectorXs du
Control disturbance.
std::size_t nr_
Dimension of the cost residual.