crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, University of Edinburgh, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
10 #define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
11 
12 #include <boost/function.hpp>
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/contact-base.hpp"
15 
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
19 class ContactModelNumDiffTpl : public ContactModelAbstractTpl<_Scalar> {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
24  typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
25  typedef ContactModelAbstractTpl<Scalar> Base;
26  typedef ContactDataNumDiffTpl<Scalar> Data;
27  typedef MathBaseTpl<Scalar> MathBase;
28  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
29  typedef boost::function<void(const typename MathBaseTpl<Scalar>::VectorXs&)> ReevaluationFunction;
30 
36  explicit ContactModelNumDiffTpl(const boost::shared_ptr<Base>& model);
37 
41  virtual ~ContactModelNumDiffTpl();
42 
46  void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
47 
51  void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
52 
56  void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
57 
64  boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
65 
71  const boost::shared_ptr<Base>& get_model() const;
72 
78  const Scalar& get_disturbance() const;
79 
85  void set_disturbance(const Scalar& disturbance);
86 
93  void set_reevals(const std::vector<ReevaluationFunction>& reevals);
94 
95  protected:
96  using Base::nc_;
97  using Base::nu_;
98  using Base::state_;
99 
101  boost::shared_ptr<Base> model_;
102 
104  Scalar disturbance_;
105 
107  std::vector<ReevaluationFunction> reevals_;
108 
109  private:
121  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
122 };
123 
124 template <typename _Scalar>
125 struct ContactDataNumDiffTpl : public ContactDataAbstractTpl<_Scalar> {
126  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
127 
128  typedef _Scalar Scalar;
131  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
132 
133  template <template <typename Scalar> class Model>
134  explicit ContactDataNumDiffTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
135  : Base(model, data), dx(model->get_state()->get_ndx()), xp(model->get_state()->get_nx()) {
136  dx.setZero();
137  xp.setZero();
138 
139  const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
140  data_0 = model->get_model()->createData(data);
141  for (std::size_t i = 0; i < ndx; ++i) {
142  data_x.push_back(model->get_model()->createData(data));
143  }
144  }
145 
146  virtual ~ContactDataNumDiffTpl() {}
147 
148  using Base::a0;
149  using Base::da0_dx;
150  using Base::f;
151  using Base::pinocchio;
152 
153  VectorXs dx;
154  VectorXs xp;
155  boost::shared_ptr<Base> data_0;
156  std::vector<boost::shared_ptr<Base> > data_x;
157 };
158 
159 } // namespace crocoddyl
160 
161 /* --- Details -------------------------------------------------------------- */
162 /* --- Details -------------------------------------------------------------- */
163 /* --- Details -------------------------------------------------------------- */
164 #include "crocoddyl/multibody/numdiff/contact.hxx"
165 
166 #endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
virtual ~ContactModelNumDiffTpl()
Default destructor of the ContactModelNumDiff object.
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
boost::shared_ptr< Base > model_
Model of the Contact.
Definition: contact.hpp:101
void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: contact.hpp:156
void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
ContactModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Construct a new ContactModelNumDiff object from a ContactModelAbstract.
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: contact.hpp:104
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: contact.hpp:107
VectorXs dx
State disturbance.
Definition: contact.hpp:153
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that take a pinocchio model, a pinocchio data, a state and a control. The updated data is used to evaluate of the gradient and hessian.
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: contact.hpp:155
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: contact.hpp:154
void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
void set_disturbance(const Scalar &disturbance)
Set the disturbance_ object.
const Scalar & get_disturbance() const
Get the disturbance_ object.
boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create a Data object.