crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 VectorXs&, const 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;
129  typedef MathBaseTpl<Scalar> MathBase;
130  typedef ContactDataAbstractTpl<Scalar> Base;
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_
crocoddyl::ContactModelNumDiffTpl::get_model
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
crocoddyl::ContactModelNumDiffTpl::model_
boost::shared_ptr< Base > model_
Model of the Contact.
Definition: contact.hpp:101
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ContactModelNumDiffTpl::updateForce
void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
crocoddyl::ContactDataNumDiffTpl::dx
VectorXs dx
State disturbance.
Definition: contact.hpp:153
crocoddyl::ContactModelNumDiffTpl::calc
void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
crocoddyl::ContactDataNumDiffTpl::data_x
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: contact.hpp:156
crocoddyl::ContactModelNumDiffTpl::ContactModelNumDiffTpl
ContactModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Construct a new ContactModelNumDiff object from a ContactModelAbstract.
crocoddyl::ContactModelNumDiffTpl::disturbance_
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: contact.hpp:104
crocoddyl::ContactModelNumDiffTpl::reevals_
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: contact.hpp:107
crocoddyl::ContactModelNumDiffTpl::set_disturbance
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
crocoddyl::ContactModelNumDiffTpl::get_disturbance
const Scalar get_disturbance() const
Get the disturbance_ object.
crocoddyl::ContactModelNumDiffTpl::set_reevals
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that take a pinocchio model, a pinocchio data, a state and a control....
crocoddyl::ForceDataAbstractTpl::f
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
crocoddyl::ContactDataAbstractTpl
Definition: contact-base.hpp:82
crocoddyl::ForceDataAbstractTpl::pinocchio
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44
crocoddyl::ContactDataNumDiffTpl
Definition: fwd.hpp:63
crocoddyl::ContactDataNumDiffTpl::xp
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: contact.hpp:154
crocoddyl::ContactModelNumDiffTpl::calcDiff
void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
crocoddyl::ContactDataNumDiffTpl::data_0
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: contact.hpp:155
crocoddyl::ContactModelNumDiffTpl::createData
boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create a Data object.
crocoddyl::ContactModelNumDiffTpl::~ContactModelNumDiffTpl
virtual ~ContactModelNumDiffTpl()
Default destructor of the ContactModelNumDiff object.