9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/numdiff/contact.hpp" 14 template <
typename Scalar>
15 ContactModelNumDiffTpl<Scalar>::ContactModelNumDiffTpl(
const boost::shared_ptr<Base>& model)
16 : Base(model->get_state(), model->get_nc(), model->get_nu()), model_(model) {
17 disturbance_ = std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon());
20 template <
typename Scalar>
21 ContactModelNumDiffTpl<Scalar>::~ContactModelNumDiffTpl() {}
23 template <
typename Scalar>
24 void ContactModelNumDiffTpl<Scalar>::calc(
const boost::shared_ptr<ContactDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>& x) {
26 boost::shared_ptr<ContactDataNumDiffTpl<Scalar> > data_nd =
27 boost::static_pointer_cast<ContactDataNumDiffTpl<Scalar> >(data);
28 model_->calc(data_nd->data_0, x);
29 data_nd->a0 = data_nd->data_0->a0;
32 template <
typename Scalar>
33 void ContactModelNumDiffTpl<Scalar>::calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
34 const Eigen::Ref<const VectorXs>& x) {
35 boost::shared_ptr<ContactDataNumDiffTpl<Scalar> > data_nd =
36 boost::static_pointer_cast<ContactDataNumDiffTpl<Scalar> >(data);
38 const VectorXs& a0 = data_nd->a0;
40 assertStableStateFD(x);
43 data_nd->dx.setZero();
44 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
46 data_nd->dx(ix) = disturbance_;
47 model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
49 for (
size_t i = 0; i < reevals_.size(); ++i) {
50 reevals_[i](data_nd->xp);
53 model_->calc(data_nd->data_x[ix], data_nd->xp);
54 data_nd->da0_dx.col(ix) = (data_nd->data_x[ix]->a0 - a0) / disturbance_;
55 data_nd->dx(ix) = 0.0;
59 template <
typename Scalar>
60 void ContactModelNumDiffTpl<Scalar>::updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
61 const VectorXs& force) {
62 if (static_cast<std::size_t>(force.size()) != model_->get_nc()) {
63 throw_pretty(
"Invalid argument: " 64 <<
"lambda has wrong dimension (it should be " << model_->get_nc() <<
")");
67 boost::shared_ptr<ContactDataNumDiffTpl<Scalar> > data_nd =
68 boost::static_pointer_cast<ContactDataNumDiffTpl<Scalar> >(data);
70 model_->updateForce(data_nd->data_0, force);
73 template <
typename Scalar>
74 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > ContactModelNumDiffTpl<Scalar>::createData(
75 pinocchio::DataTpl<Scalar>*
const data) {
76 return boost::make_shared<ContactDataNumDiffTpl<Scalar> >(
this, data);
79 template <
typename Scalar>
80 const boost::shared_ptr<ContactModelAbstractTpl<Scalar> >& ContactModelNumDiffTpl<Scalar>::get_model()
const {
84 template <
typename Scalar>
85 const Scalar& ContactModelNumDiffTpl<Scalar>::get_disturbance()
const {
89 template <
typename Scalar>
90 void ContactModelNumDiffTpl<Scalar>::set_disturbance(
const Scalar& disturbance) {
91 disturbance_ = disturbance;
94 template <
typename Scalar>
95 void ContactModelNumDiffTpl<Scalar>::set_reevals(
const std::vector<ReevaluationFunction>& reevals) {
99 template <
typename Scalar>
100 void ContactModelNumDiffTpl<Scalar>::assertStableStateFD(
const Eigen::Ref<const VectorXs>& ) {
Definition: action-base.hxx:11