1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, University of Edinburgh, LAAS-CNRS, |
5 |
|
|
// Heriot-Watt University |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#ifndef CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_ |
11 |
|
|
#define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_ |
12 |
|
|
|
13 |
|
|
#include <boost/function.hpp> |
14 |
|
|
|
15 |
|
|
#include "crocoddyl/multibody/contact-base.hpp" |
16 |
|
|
#include "crocoddyl/multibody/fwd.hpp" |
17 |
|
|
|
18 |
|
|
namespace crocoddyl { |
19 |
|
|
|
20 |
|
|
template <typename _Scalar> |
21 |
|
|
class ContactModelNumDiffTpl : public ContactModelAbstractTpl<_Scalar> { |
22 |
|
|
public: |
23 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
24 |
|
|
|
25 |
|
|
typedef _Scalar Scalar; |
26 |
|
|
typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract; |
27 |
|
|
typedef ContactModelAbstractTpl<Scalar> Base; |
28 |
|
|
typedef ContactDataNumDiffTpl<Scalar> Data; |
29 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
30 |
|
|
typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs; |
31 |
|
|
typedef boost::function<void(const VectorXs&, const VectorXs&)> |
32 |
|
|
ReevaluationFunction; |
33 |
|
|
|
34 |
|
|
/** |
35 |
|
|
* @brief Construct a new ContactModelNumDiff object from a |
36 |
|
|
* ContactModelAbstract. |
37 |
|
|
* |
38 |
|
|
* @param model |
39 |
|
|
*/ |
40 |
|
|
explicit ContactModelNumDiffTpl(const boost::shared_ptr<Base>& model); |
41 |
|
|
|
42 |
|
|
/** |
43 |
|
|
* @brief Default destructor of the ContactModelNumDiff object |
44 |
|
|
*/ |
45 |
|
|
virtual ~ContactModelNumDiffTpl(); |
46 |
|
|
|
47 |
|
|
/** |
48 |
|
|
* @brief @copydoc ContactModelAbstract::calc() |
49 |
|
|
*/ |
50 |
|
|
void calc(const boost::shared_ptr<ContactDataAbstract>& data, |
51 |
|
|
const Eigen::Ref<const VectorXs>& x); |
52 |
|
|
|
53 |
|
|
/** |
54 |
|
|
* @brief @copydoc ContactModelAbstract::calcDiff() |
55 |
|
|
*/ |
56 |
|
|
void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, |
57 |
|
|
const Eigen::Ref<const VectorXs>& x); |
58 |
|
|
|
59 |
|
|
/** |
60 |
|
|
* @brief @copydoc ContactModelAbstract::updateForce() |
61 |
|
|
*/ |
62 |
|
|
void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, |
63 |
|
|
const VectorXs& force); |
64 |
|
|
|
65 |
|
|
/** |
66 |
|
|
* @brief Create a Data object |
67 |
|
|
* |
68 |
|
|
* @param data is the Pinocchio data |
69 |
|
|
* @return boost::shared_ptr<ContactModelAbstract> |
70 |
|
|
*/ |
71 |
|
|
boost::shared_ptr<ContactDataAbstract> createData( |
72 |
|
|
pinocchio::DataTpl<Scalar>* const data); |
73 |
|
|
|
74 |
|
|
/** |
75 |
|
|
* @brief Return the acton model that we use to numerical differentiate |
76 |
|
|
*/ |
77 |
|
|
const boost::shared_ptr<Base>& get_model() const; |
78 |
|
|
|
79 |
|
|
/** |
80 |
|
|
* @brief Return the disturbance constant used in the numerical |
81 |
|
|
* differentiation routine |
82 |
|
|
*/ |
83 |
|
|
const Scalar get_disturbance() const; |
84 |
|
|
|
85 |
|
|
/** |
86 |
|
|
* @brief Modify the disturbance constant used in the numerical |
87 |
|
|
* differentiation routine |
88 |
|
|
*/ |
89 |
|
|
void set_disturbance(const Scalar disturbance); |
90 |
|
|
|
91 |
|
|
/** |
92 |
|
|
* @brief Register functions that take a pinocchio model, a pinocchio data, a |
93 |
|
|
* state and a control. The updated data is used to evaluate of the gradient |
94 |
|
|
* and Hessian. |
95 |
|
|
* |
96 |
|
|
* @param reevals are the registered functions. |
97 |
|
|
*/ |
98 |
|
|
void set_reevals(const std::vector<ReevaluationFunction>& reevals); |
99 |
|
|
|
100 |
|
|
protected: |
101 |
|
|
using Base::nc_; |
102 |
|
|
using Base::nu_; |
103 |
|
|
using Base::state_; |
104 |
|
|
|
105 |
|
|
boost::shared_ptr<Base> model_; //!< contact model to differentiate |
106 |
|
|
Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian |
107 |
|
|
//!< calculation |
108 |
|
|
std::vector<ReevaluationFunction> |
109 |
|
|
reevals_; //!< functions that need execution before calc or calcDiff |
110 |
|
|
|
111 |
|
|
private: |
112 |
|
|
/** |
113 |
|
|
* @brief Make sure that when we finite difference the Cost Model, the user |
114 |
|
|
* does not face unknown behaviour because of the finite differencing of a |
115 |
|
|
* quaternion around pi. This behaviour might occur if state cost in |
116 |
|
|
* floating systems. |
117 |
|
|
* |
118 |
|
|
* For full discussions see issue |
119 |
|
|
* https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139 |
120 |
|
|
* |
121 |
|
|
* @param x is the state at which the check is performed. |
122 |
|
|
*/ |
123 |
|
|
void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/); |
124 |
|
|
}; |
125 |
|
|
|
126 |
|
|
template <typename _Scalar> |
127 |
|
|
struct ContactDataNumDiffTpl : public ContactDataAbstractTpl<_Scalar> { |
128 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
129 |
|
|
|
130 |
|
|
typedef _Scalar Scalar; |
131 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
132 |
|
|
typedef ContactDataAbstractTpl<Scalar> Base; |
133 |
|
|
typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs; |
134 |
|
|
|
135 |
|
|
template <template <typename Scalar> class Model> |
136 |
|
50 |
explicit ContactDataNumDiffTpl(Model<Scalar>* const model, |
137 |
|
|
pinocchio::DataTpl<Scalar>* const data) |
138 |
|
|
: Base(model, data), |
139 |
|
50 |
dx(model->get_state()->get_ndx()), |
140 |
✓✗✓✗
|
100 |
xp(model->get_state()->get_nx()) { |
141 |
✓✗ |
50 |
dx.setZero(); |
142 |
✓✗ |
50 |
xp.setZero(); |
143 |
|
|
|
144 |
|
50 |
const std::size_t ndx = model->get_model()->get_state()->get_ndx(); |
145 |
✓✗ |
50 |
data_0 = model->get_model()->createData(data); |
146 |
✓✓ |
2070 |
for (std::size_t i = 0; i < ndx; ++i) { |
147 |
✓✗✓✗
|
2020 |
data_x.push_back(model->get_model()->createData(data)); |
148 |
|
|
} |
149 |
|
50 |
} |
150 |
|
|
|
151 |
|
100 |
virtual ~ContactDataNumDiffTpl() {} |
152 |
|
|
|
153 |
|
|
using Base::a0; |
154 |
|
|
using Base::da0_dx; |
155 |
|
|
using Base::f; |
156 |
|
|
using Base::pinocchio; |
157 |
|
|
|
158 |
|
|
Scalar x_norm; //!< Norm of the state vector |
159 |
|
|
Scalar |
160 |
|
|
xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$ |
161 |
|
|
VectorXs dx; //!< State disturbance. |
162 |
|
|
VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$ |
163 |
|
|
//!< \int x dx_i \f$". |
164 |
|
|
boost::shared_ptr<Base> data_0; //!< The data at the approximation point. |
165 |
|
|
std::vector<boost::shared_ptr<Base> > |
166 |
|
|
data_x; //!< The temporary data associated with the state variation. |
167 |
|
|
}; |
168 |
|
|
|
169 |
|
|
} // namespace crocoddyl |
170 |
|
|
|
171 |
|
|
/* --- Details -------------------------------------------------------------- */ |
172 |
|
|
/* --- Details -------------------------------------------------------------- */ |
173 |
|
|
/* --- Details -------------------------------------------------------------- */ |
174 |
|
|
#include "crocoddyl/multibody/numdiff/contact.hxx" |
175 |
|
|
|
176 |
|
|
#endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_ |