contact.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/numdiff/contact.hpp"
11 
12 namespace crocoddyl {
13 
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());
18 }
19 
20 template <typename Scalar>
21 ContactModelNumDiffTpl<Scalar>::~ContactModelNumDiffTpl() {}
22 
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;
30 }
31 
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);
37 
38  const VectorXs& a0 = data_nd->a0;
39 
40  assertStableStateFD(x);
41 
42  // Computing the d contact(x,u) / dx
43  data_nd->dx.setZero();
44  for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
45  // x + dx
46  data_nd->dx(ix) = disturbance_;
47  model_->get_state()->integrate(x, data_nd->dx, data_nd->xp);
48  // call the update function on the pinocchio data
49  for (size_t i = 0; i < reevals_.size(); ++i) {
50  reevals_[i](data_nd->xp);
51  }
52  // contact(x+dx, u)
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;
56  }
57 }
58 
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() << ")");
65  }
66 
67  boost::shared_ptr<ContactDataNumDiffTpl<Scalar> > data_nd =
68  boost::static_pointer_cast<ContactDataNumDiffTpl<Scalar> >(data);
69 
70  model_->updateForce(data_nd->data_0, force);
71 }
72 
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);
77 }
78 
79 template <typename Scalar>
80 const boost::shared_ptr<ContactModelAbstractTpl<Scalar> >& ContactModelNumDiffTpl<Scalar>::get_model() const {
81  return model_;
82 }
83 
84 template <typename Scalar>
85 const Scalar& ContactModelNumDiffTpl<Scalar>::get_disturbance() const {
86  return disturbance_;
87 }
88 
89 template <typename Scalar>
90 void ContactModelNumDiffTpl<Scalar>::set_disturbance(const Scalar& disturbance) {
91  disturbance_ = disturbance;
92 }
93 
94 template <typename Scalar>
95 void ContactModelNumDiffTpl<Scalar>::set_reevals(const std::vector<ReevaluationFunction>& reevals) {
96  reevals_ = reevals;
97 }
98 
99 template <typename Scalar>
100 void ContactModelNumDiffTpl<Scalar>::assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/) {
101  // do nothing in the general case
102 }
103 
104 } // namespace crocoddyl
Definition: action-base.hxx:11