GCC Code Coverage Report

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