9 #ifndef CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_ 10 #define CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_ 12 #include <boost/function.hpp> 13 #include "crocoddyl/multibody/fwd.hpp" 14 #include "crocoddyl/core/residual-base.hpp" 18 template <
typename _Scalar>
19 class ResidualModelNumDiffTpl :
public ResidualModelAbstractTpl<_Scalar> {
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
24 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
25 typedef ResidualModelAbstractTpl<Scalar> Base;
26 typedef ResidualDataNumDiffTpl<Scalar> Data;
27 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
28 typedef MathBaseTpl<Scalar> MathBase;
29 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
30 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
31 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
48 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
49 const Eigen::Ref<const VectorXs>& u);
54 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
55 const Eigen::Ref<const VectorXs>& u);
63 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
68 const boost::shared_ptr<Base>&
get_model()
const;
86 void set_reevals(
const std::vector<ReevaluationFunction>& reevals);
114 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& );
117 template <
typename _Scalar>
119 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
121 typedef _Scalar Scalar;
128 template <
template <
typename Scalar>
class Model>
130 : Base(model, shared_data),
140 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
141 const std::size_t& nu = model->get_model()->get_nu();
142 data_0 = model->get_model()->createData(shared_data);
143 for (std::size_t i = 0; i < ndx; ++i) {
144 data_x.push_back(model->get_model()->createData(shared_data));
146 for (std::size_t i = 0; i < nu; ++i) {
147 data_u.push_back(model->get_model()->createData(shared_data));
151 virtual ~ResidualDataNumDiffTpl() {}
163 std::vector<boost::shared_ptr<Base> >
data_x;
164 std::vector<boost::shared_ptr<Base> >
data_u;
172 #include "crocoddyl/core/numdiff/residual.hxx" 174 #endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_ VectorXs du
Control disturbance.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create a data object.
virtual ~ResidualModelNumDiffTpl()
Initialize the numdiff residual model.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the residual vector.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
boost::shared_ptr< Base > model_
Model of the residual.
Abstract class for residual models.
std::size_t nu_
Control dimension.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian of the residual vector.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
boost::shared_ptr< Base > data_0
The data at the approximation point.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
ResidualModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff residual model.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t get_nu() const
Return the dimension of the control input.
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
VectorXs dx
State disturbance.
VectorXs unone_
No control vector.
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that updates the shared data computed for a system rollout The updated data is use...
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.