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" 27 template <
typename _Scalar>
28 class ResidualModelNumDiffTpl :
public ResidualModelAbstractTpl<_Scalar> {
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 typedef _Scalar Scalar;
33 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
34 typedef ResidualModelAbstractTpl<Scalar> Base;
35 typedef ResidualDataNumDiffTpl<Scalar> Data;
36 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
37 typedef MathBaseTpl<Scalar> MathBase;
38 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
39 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
40 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
57 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
58 const Eigen::Ref<const VectorXs>& u);
64 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
69 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
70 const Eigen::Ref<const VectorXs>& u);
76 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
81 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
86 const boost::shared_ptr<Base>&
get_model()
const;
104 void set_reevals(
const std::vector<ReevaluationFunction>& reevals);
123 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& );
125 boost::shared_ptr<Base> model_;
127 std::vector<ReevaluationFunction> reevals_;
130 template <
typename _Scalar>
131 struct ResidualDataNumDiffTpl :
public ResidualDataAbstractTpl<_Scalar> {
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
134 typedef _Scalar Scalar;
135 typedef MathBaseTpl<Scalar> MathBase;
136 typedef ResidualDataAbstractTpl<Scalar> Base;
137 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
138 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
139 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
147 template <
template <
typename Scalar>
class Model>
149 :
Base(model, shared_data),
150 dx(model->get_state()->get_ndx()),
151 xp(model->get_state()->get_nx()),
153 up(model->get_nu()) {
159 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
160 const std::size_t& nu = model->get_model()->get_nu();
161 data_0 = model->get_model()->createData(shared_data);
162 for (std::size_t i = 0; i < ndx; ++i) {
163 data_x.push_back(model->get_model()->createData(shared_data));
165 for (std::size_t i = 0; i < nu; ++i) {
166 data_u.push_back(model->get_model()->createData(shared_data));
182 std::vector<boost::shared_ptr<Base> >
data_x;
183 std::vector<boost::shared_ptr<Base> >
data_u;
191 #include "crocoddyl/core/numdiff/residual.hxx" 193 #endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_ VectorXs du
Control disturbance.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
virtual ~ResidualModelNumDiffTpl()
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.
ResidualDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff residual data.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
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.
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.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
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.
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.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.