10 #ifndef CROCODDYL_CORE_NUMDIFF_COST_HPP_ 11 #define CROCODDYL_CORE_NUMDIFF_COST_HPP_ 13 #include <boost/function.hpp> 14 #include "crocoddyl/multibody/fwd.hpp" 15 #include "crocoddyl/core/cost-base.hpp" 28 template <
typename _Scalar>
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 typedef _Scalar Scalar;
41 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
58 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
59 const Eigen::Ref<const VectorXs>& u);
64 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
69 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
70 const Eigen::Ref<const VectorXs>& u);
76 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
84 virtual boost::shared_ptr<CostDataAbstract>
createData(DataCollectorAbstract*
const data);
89 const boost::shared_ptr<Base>&
get_model()
const;
115 void set_reevals(
const std::vector<ReevaluationFunction>& reevals);
135 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& );
137 boost::shared_ptr<Base> model_;
139 std::vector<ReevaluationFunction> reevals_;
142 template <
typename _Scalar>
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 typedef _Scalar Scalar;
159 template <
template <
typename Scalar>
class Model>
161 : Base(model, shared_data),
171 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
172 const std::size_t nu = model->get_model()->get_nu();
173 data_0 = model->get_model()->createData(shared_data);
174 for (std::size_t i = 0; i < ndx; ++i) {
175 data_x.push_back(model->get_model()->createData(shared_data));
177 for (std::size_t i = 0; i < nu; ++i) {
178 data_u.push_back(model->get_model()->createData(shared_data));
184 using Base::activation;
191 using Base::residual;
199 std::vector<boost::shared_ptr<Base> >
data_x;
200 std::vector<boost::shared_ptr<Base> >
data_u;
208 #include "crocoddyl/core/numdiff/cost.hxx" 210 #endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_ Abstract class for cost models.
virtual ~CostModelNumDiffTpl()
Initialize the numdiff cost model.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create a numdiff cost data.
CostModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Initialize the numdiff cost model.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
void set_disturbance(const Scalar disturbance)
Modify the disturbance value used by the numdiff routine.
CostDataNumDiffTpl(Model< Scalar > *const model, DataCollectorAbstract *const shared_data)
Initialize the numdiff cost data.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
std::vector< boost::shared_ptr< Base > > data_u
The temporary data associated with the control variation.
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...
VectorXs up
The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
This class computes the numerical differentiation of a cost model.
VectorXs du
Control disturbance.
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the cost value and its residual vector.
const Scalar get_disturbance() const
Return the disturbance value used by the numdiff routine.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian and Hessian of cost and its residual vector.
const boost::shared_ptr< Base > & get_model() const
Return the original cost model.
VectorXs unone_
No control vector.
VectorXs dx
State disturbance.
boost::shared_ptr< Base > data_0
The data at the approximation point.
std::size_t nu_
Control dimension.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t get_nu() const
Return the dimension of the control input.