10 #ifndef CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_ 11 #define CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_ 13 #include <boost/function.hpp> 14 #include "crocoddyl/multibody/fwd.hpp" 15 #include "crocoddyl/multibody/cost-base.hpp" 19 template <
typename _Scalar>
20 class CostModelNumDiffTpl :
public CostModelAbstractTpl<_Scalar> {
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _Scalar Scalar;
25 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
26 typedef CostModelAbstractTpl<Scalar> Base;
27 typedef CostDataNumDiffTpl<Scalar> Data;
28 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
29 typedef MathBaseTpl<Scalar> MathBase;
30 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
31 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
32 typedef boost::function<void(const typename MathBaseTpl<Scalar>::VectorXs&)> ReevaluationFunction;
49 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
50 const Eigen::Ref<const VectorXs>& u);
55 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
56 const Eigen::Ref<const VectorXs>& u);
64 virtual boost::shared_ptr<CostDataAbstract>
createData(DataCollectorAbstract*
const data);
71 const boost::shared_ptr<Base>&
get_model()
const;
101 void set_reevals(
const std::vector<ReevaluationFunction>& reevals);
104 using Base::activation_;
130 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& );
133 template <
typename _Scalar>
135 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 typedef _Scalar Scalar;
144 template <
template <
typename Scalar>
class Model>
145 explicit CostDataNumDiffTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const shared_data)
146 : Base(model, shared_data),
147 dx(model->get_state()->get_ndx()),
148 xp(model->get_state()->get_nx()),
150 up(model->get_nu()) {
156 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
157 const std::size_t& nu = model->get_model()->get_nu();
158 data_0 = model->get_model()->createData(shared_data);
159 for (std::size_t i = 0; i < ndx; ++i) {
160 data_x.push_back(model->get_model()->createData(shared_data));
162 for (std::size_t i = 0; i < nu; ++i) {
163 data_u.push_back(model->get_model()->createData(shared_data));
167 virtual ~CostDataNumDiffTpl() {}
169 using Base::activation;
186 std::vector<boost::shared_ptr<Base> >
data_x;
187 std::vector<boost::shared_ptr<Base> >
data_u;
195 #include "crocoddyl/multibody/numdiff/cost.hxx" 197 #endif // CROCODDYL_MULTIBODY_NUMDIFF_COST_HPP_
virtual ~CostModelNumDiffTpl()
Default destructor of the CostModelNumDiff object.
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 Data object.
CostModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Construct a new CostModelNumDiff object from a CostModelAbstract.
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
const Scalar & get_disturbance() const
Get the disturbance_ object.
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$".
VectorXs du
Control disturbance.
void set_disturbance(const Scalar &disturbance)
Set the disturbance_ object.
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)
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
VectorXs dx
State disturbance.
boost::shared_ptr< Base > data_0
The data at the approximation point.
boost::shared_ptr< Base > model_
Model of the cost.