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>
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
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);
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),
131 dx(model->get_state()->get_ndx()),
132 xp(model->get_state()->get_nx()),
134 up(model->get_nu()) {
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));
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"
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
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...
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.
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.
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.
const boost::shared_ptr< Base > & get_model() const
Return the original residual model.
boost::shared_ptr< Base > model_
Model of the residual.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create a data object.
virtual ~ResidualModelNumDiffTpl()
Initialize the numdiff residual model.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
VectorXs r
Residual vector.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
boost::shared_ptr< Base > data_0
The data at the approximation point.
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.
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
VectorXs dx
State disturbance.