9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/core/numdiff/actuation.hpp" 14 template <
typename Scalar>
15 ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(boost::shared_ptr<Base> model)
16 : Base(model->get_state(), model->get_nu()), model_(model) {
17 disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
20 template <
typename Scalar>
21 ActuationModelNumDiffTpl<Scalar>::~ActuationModelNumDiffTpl() {}
23 template <
typename Scalar>
24 void ActuationModelNumDiffTpl<Scalar>::calc(
const boost::shared_ptr<ActuationDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
26 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
27 throw_pretty(
"Invalid argument: " 28 <<
"x has wrong dimension (it should be " + std::to_string(model_->get_state()->get_nx()) +
")");
30 if (static_cast<std::size_t>(u.size()) != nu_) {
31 throw_pretty(
"Invalid argument: " 32 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
34 boost::shared_ptr<ActuationDataNumDiffTpl<Scalar> > data_nd =
35 boost::static_pointer_cast<ActuationDataNumDiffTpl<Scalar> >(data);
36 model_->calc(data_nd->data_0, x, u);
37 data->tau = data_nd->data_0->tau;
40 template <
typename Scalar>
41 void ActuationModelNumDiffTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActuationDataAbstract>& data,
42 const Eigen::Ref<const VectorXs>& x,
43 const Eigen::Ref<const VectorXs>& u) {
44 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
45 throw_pretty(
"Invalid argument: " 46 <<
"x has wrong dimension (it should be " + std::to_string(model_->get_state()->get_nx()) +
")");
48 if (static_cast<std::size_t>(u.size()) != nu_) {
49 throw_pretty(
"Invalid argument: " 50 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
52 boost::shared_ptr<ActuationDataNumDiffTpl<Scalar> > data_nd =
53 boost::static_pointer_cast<ActuationDataNumDiffTpl<Scalar> >(data);
55 const VectorXs& tau0 = data_nd->data_0->tau;
58 data_nd->dx.setZero();
59 for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
60 data_nd->dx(ix) = disturbance_;
61 model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
62 model_->calc(data_nd->data_x[ix], data_nd->xp, u);
63 data_nd->dtau_dx.col(ix) = (data_nd->data_x[ix]->tau - tau0) / disturbance_;
64 data_nd->dx(ix) = 0.0;
68 data_nd->du.setZero();
69 for (
unsigned iu = 0; iu < model_->get_nu(); ++iu) {
70 data_nd->du(iu) = disturbance_;
71 model_->calc(data_nd->data_u[iu], x, u + data_nd->du);
72 data_nd->dtau_du.col(iu) = (data_nd->data_u[iu]->tau - tau0) / disturbance_;
73 data_nd->du(iu) = 0.0;
77 template <
typename Scalar>
78 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > ActuationModelNumDiffTpl<Scalar>::createData() {
79 return boost::make_shared<ActuationDataNumDiffTpl<Scalar> >(
this);
82 template <
typename Scalar>
83 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >& ActuationModelNumDiffTpl<Scalar>::get_model()
const {
87 template <
typename Scalar>
88 const Scalar& ActuationModelNumDiffTpl<Scalar>::get_disturbance()
const {
92 template <
typename Scalar>
93 void ActuationModelNumDiffTpl<Scalar>::set_disturbance(
const Scalar& disturbance) {
94 if (disturbance < 0.) {
95 throw_pretty(
"Invalid argument: " 96 <<
"Disturbance value is positive");
98 disturbance_ = disturbance;
Definition: action-base.hxx:11