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"
51 template <
typename _Scalar>
54 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56 typedef _Scalar Scalar;
79 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u);
85 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
86 const Eigen::Ref<const VectorXs>& u);
93 virtual boost::shared_ptr<ActionDataAbstract>
createData();
145 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
151 boost::shared_ptr<Base> model_;
159 bool with_gauss_approx_;
162 template <
typename _Scalar>
164 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166 typedef _Scalar Scalar;
178 template <
template <
typename Scalar>
class Model>
181 Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
182 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
183 dx(model->get_model()->get_state()->get_ndx()),
184 du(model->get_model()->get_nu()),
185 xp(model->get_model()->get_state()->get_nx()) {
192 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
193 const std::size_t nu = model->get_model()->get_nu();
194 data_0 = model->get_model()->createData();
195 for (std::size_t i = 0; i < ndx; ++i) {
196 data_x.push_back(model->get_model()->createData());
198 for (std::size_t i = 0; i < nu; ++i) {
199 data_u.push_back(model->get_model()->createData());
220 std::vector<boost::shared_ptr<Base> >
data_x;
221 std::vector<boost::shared_ptr<Base> >
data_u;
229 #include "crocoddyl/core/numdiff/action.hxx"
Abstract class for action model.
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of an ActionModel.
const Scalar get_disturbance() const
Get the disturbance_ object.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
ActionModelNumDiffTpl(boost::shared_ptr< Base > model, bool with_gauss_approx=false)
Construct a new ActionModelNumDiff object.
virtual ~ActionModelNumDiffTpl()
Destroy the ActionModelNumDiff object.
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.
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.
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create a Data object from the given model.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
MatrixXs Ru
Cost residual jacobian: .
MatrixXs Rx
Cost residual jacobian: .
boost::shared_ptr< Base > data_0
The data that contains the final results.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
VectorXs du
Control disturbance.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
ActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
VectorXs dx
State disturbance.