1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, New York |
5 |
|
|
// University, |
6 |
|
|
// Max Planck Gesellschaft, |
7 |
|
|
// Heriot-Watt University |
8 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
9 |
|
|
// All rights reserved. |
10 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
11 |
|
|
|
12 |
|
|
#ifndef CROCODDYL_CORE_NUMDIFF_COST_HPP_ |
13 |
|
|
#define CROCODDYL_CORE_NUMDIFF_COST_HPP_ |
14 |
|
|
|
15 |
|
|
#include <boost/function.hpp> |
16 |
|
|
|
17 |
|
|
#include "crocoddyl/core/cost-base.hpp" |
18 |
|
|
#include "crocoddyl/multibody/fwd.hpp" |
19 |
|
|
|
20 |
|
|
namespace crocoddyl { |
21 |
|
|
|
22 |
|
|
/** |
23 |
|
|
* @brief This class computes the numerical differentiation of a cost model. |
24 |
|
|
* |
25 |
|
|
* It computes the Jacobian and Hessian of the cost model via numerical |
26 |
|
|
* differentiation, i.e., \f$\mathbf{\ell_x}\f$, \f$\mathbf{\ell_u}\f$, |
27 |
|
|
* \f$\mathbf{\ell_{xx}}\f$, \f$\mathbf{\ell_{uu}}\f$, and |
28 |
|
|
* \f$\mathbf{\ell_{xu}}\f$ which denote the Jacobians and Hessians of the cost |
29 |
|
|
* function, respectively. |
30 |
|
|
* |
31 |
|
|
* \sa `CostModelAbstractTpl()`, `calcDiff()` |
32 |
|
|
*/ |
33 |
|
|
template <typename _Scalar> |
34 |
|
|
class CostModelNumDiffTpl : public CostModelAbstractTpl<_Scalar> { |
35 |
|
|
public: |
36 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
37 |
|
|
|
38 |
|
|
typedef _Scalar Scalar; |
39 |
|
|
typedef CostDataAbstractTpl<Scalar> CostDataAbstract; |
40 |
|
|
typedef CostModelAbstractTpl<Scalar> Base; |
41 |
|
|
typedef CostDataNumDiffTpl<Scalar> Data; |
42 |
|
|
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; |
43 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
44 |
|
|
typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs; |
45 |
|
|
typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs; |
46 |
|
|
typedef boost::function<void(const VectorXs&, const VectorXs&)> |
47 |
|
|
ReevaluationFunction; |
48 |
|
|
|
49 |
|
|
/** |
50 |
|
|
* @brief Initialize the numdiff cost model |
51 |
|
|
* |
52 |
|
|
* @param model Cost model that we want to apply the numerical |
53 |
|
|
* differentiation |
54 |
|
|
*/ |
55 |
|
|
explicit CostModelNumDiffTpl(const boost::shared_ptr<Base>& model); |
56 |
|
|
|
57 |
|
|
/** |
58 |
|
|
* @brief Initialize the numdiff cost model |
59 |
|
|
*/ |
60 |
|
|
virtual ~CostModelNumDiffTpl(); |
61 |
|
|
|
62 |
|
|
/** |
63 |
|
|
* @brief @copydoc Base::calc() |
64 |
|
|
*/ |
65 |
|
|
virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, |
66 |
|
|
const Eigen::Ref<const VectorXs>& x, |
67 |
|
|
const Eigen::Ref<const VectorXs>& u); |
68 |
|
|
|
69 |
|
|
/** |
70 |
|
|
* @brief @copydoc Base::calc(const boost::shared_ptr<CostDataAbstract>& data, |
71 |
|
|
* const Eigen::Ref<const VectorXs>& x) |
72 |
|
|
*/ |
73 |
|
|
virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, |
74 |
|
|
const Eigen::Ref<const VectorXs>& x); |
75 |
|
|
|
76 |
|
|
/** |
77 |
|
|
* @brief @copydoc Base::calcDiff() |
78 |
|
|
*/ |
79 |
|
|
virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, |
80 |
|
|
const Eigen::Ref<const VectorXs>& x, |
81 |
|
|
const Eigen::Ref<const VectorXs>& u); |
82 |
|
|
|
83 |
|
|
/** |
84 |
|
|
* @brief @copydoc Base::calcDiff(const boost::shared_ptr<CostDataAbstract>& |
85 |
|
|
* data, const Eigen::Ref<const VectorXs>& x) |
86 |
|
|
*/ |
87 |
|
|
virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, |
88 |
|
|
const Eigen::Ref<const VectorXs>& x); |
89 |
|
|
|
90 |
|
|
/** |
91 |
|
|
* @brief Create a numdiff cost data |
92 |
|
|
* |
93 |
|
|
* @param data Data collector used by the original model |
94 |
|
|
* @return the numdiff cost data |
95 |
|
|
*/ |
96 |
|
|
virtual boost::shared_ptr<CostDataAbstract> createData( |
97 |
|
|
DataCollectorAbstract* const data); |
98 |
|
|
|
99 |
|
|
/** |
100 |
|
|
* @brief Return the original cost model |
101 |
|
|
*/ |
102 |
|
|
const boost::shared_ptr<Base>& get_model() const; |
103 |
|
|
|
104 |
|
|
/** |
105 |
|
|
* @brief Return the disturbance constant used by the numerical |
106 |
|
|
* differentiation routine |
107 |
|
|
*/ |
108 |
|
|
const Scalar get_disturbance() const; |
109 |
|
|
|
110 |
|
|
/** |
111 |
|
|
* @brief Modify the disturbance constant used by the numerical |
112 |
|
|
* differentiation routine |
113 |
|
|
*/ |
114 |
|
|
void set_disturbance(const Scalar disturbance); |
115 |
|
|
|
116 |
|
|
/** |
117 |
|
|
* @brief Identify if the Gauss approximation is going to be used or not. |
118 |
|
|
* |
119 |
|
|
* @return true |
120 |
|
|
* @return false |
121 |
|
|
*/ |
122 |
|
|
bool get_with_gauss_approx(); |
123 |
|
|
|
124 |
|
|
/** |
125 |
|
|
* @brief Register functions that updates the shared data computed for a |
126 |
|
|
* system rollout The updated data is used to evaluate of the gradient and |
127 |
|
|
* Hessian. |
128 |
|
|
* |
129 |
|
|
* @param reevals are the registered functions. |
130 |
|
|
*/ |
131 |
|
|
void set_reevals(const std::vector<ReevaluationFunction>& reevals); |
132 |
|
|
|
133 |
|
|
protected: |
134 |
|
|
using Base::activation_; |
135 |
|
|
using Base::nu_; |
136 |
|
|
using Base::state_; |
137 |
|
|
using Base::unone_; |
138 |
|
|
|
139 |
|
|
private: |
140 |
|
|
/** |
141 |
|
|
* @brief Make sure that when we finite difference the Cost Model, the user |
142 |
|
|
* does not face unknown behaviour because of the finite differencing of a |
143 |
|
|
* quaternion around pi. This behaviour might occur if state cost in and |
144 |
|
|
* floating systems. |
145 |
|
|
* |
146 |
|
|
* For full discussions see issue |
147 |
|
|
* https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139 |
148 |
|
|
* |
149 |
|
|
* @param x is the state at which the check is performed. |
150 |
|
|
*/ |
151 |
|
|
void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/); |
152 |
|
|
|
153 |
|
|
boost::shared_ptr<Base> model_; //!< Cost model hat we want to apply the |
154 |
|
|
//!< numerical differentiation |
155 |
|
|
Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian |
156 |
|
|
//!< calculation |
157 |
|
|
std::vector<ReevaluationFunction> |
158 |
|
|
reevals_; //!< Functions that needs execution before calc or calcDiff |
159 |
|
|
}; |
160 |
|
|
|
161 |
|
|
template <typename _Scalar> |
162 |
|
|
struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> { |
163 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
164 |
|
|
|
165 |
|
|
typedef _Scalar Scalar; |
166 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
167 |
|
|
typedef CostDataAbstractTpl<Scalar> Base; |
168 |
|
|
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; |
169 |
|
|
typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract; |
170 |
|
|
typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs; |
171 |
|
|
|
172 |
|
|
/** |
173 |
|
|
* @brief Initialize the numdiff cost data |
174 |
|
|
* |
175 |
|
|
* @tparam Model is the type of the `CostModelAbstractTpl`. |
176 |
|
|
* @param model is the object to compute the numerical differentiation from. |
177 |
|
|
*/ |
178 |
|
|
template <template <typename Scalar> class Model> |
179 |
|
652 |
explicit CostDataNumDiffTpl(Model<Scalar>* const model, |
180 |
|
|
DataCollectorAbstract* const shared_data) |
181 |
|
|
: Base(model, shared_data), |
182 |
|
652 |
dx(model->get_state()->get_ndx()), |
183 |
|
652 |
xp(model->get_state()->get_nx()), |
184 |
|
|
du(model->get_nu()), |
185 |
✓✗✓✗ ✓✗✓✗
|
652 |
up(model->get_nu()) { |
186 |
✓✗ |
652 |
dx.setZero(); |
187 |
✓✗ |
652 |
xp.setZero(); |
188 |
✓✗ |
652 |
du.setZero(); |
189 |
✓✗ |
652 |
up.setZero(); |
190 |
|
|
|
191 |
|
652 |
const std::size_t ndx = model->get_model()->get_state()->get_ndx(); |
192 |
|
652 |
const std::size_t nu = model->get_model()->get_nu(); |
193 |
✓✗ |
652 |
data_0 = model->get_model()->createData(shared_data); |
194 |
✓✓ |
26808 |
for (std::size_t i = 0; i < ndx; ++i) { |
195 |
✓✗✓✗
|
26156 |
data_x.push_back(model->get_model()->createData(shared_data)); |
196 |
|
|
} |
197 |
✓✓ |
13730 |
for (std::size_t i = 0; i < nu; ++i) { |
198 |
✓✗✓✗
|
13078 |
data_u.push_back(model->get_model()->createData(shared_data)); |
199 |
|
|
} |
200 |
|
652 |
} |
201 |
|
|
|
202 |
|
1304 |
virtual ~CostDataNumDiffTpl() {} |
203 |
|
|
|
204 |
|
|
using Base::activation; |
205 |
|
|
using Base::cost; |
206 |
|
|
using Base::Lu; |
207 |
|
|
using Base::Luu; |
208 |
|
|
using Base::Lx; |
209 |
|
|
using Base::Lxu; |
210 |
|
|
using Base::Lxx; |
211 |
|
|
using Base::residual; |
212 |
|
|
using Base::shared; |
213 |
|
|
|
214 |
|
|
Scalar x_norm; //!< Norm of the state vector |
215 |
|
|
Scalar |
216 |
|
|
xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$ |
217 |
|
|
Scalar |
218 |
|
|
uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$ |
219 |
|
|
VectorXs dx; //!< State disturbance. |
220 |
|
|
VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$ |
221 |
|
|
//!< \int x dx_i \f$". |
222 |
|
|
VectorXs du; //!< Control disturbance. |
223 |
|
|
VectorXs up; //!< The integrated control from the disturbance on one DoF "\f$ |
224 |
|
|
//!< \int u du_i = u + du \f$". |
225 |
|
|
boost::shared_ptr<Base> data_0; //!< The data at the approximation point. |
226 |
|
|
std::vector<boost::shared_ptr<Base> > |
227 |
|
|
data_x; //!< The temporary data associated with the state variation. |
228 |
|
|
std::vector<boost::shared_ptr<Base> > |
229 |
|
|
data_u; //!< The temporary data associated with the control variation. |
230 |
|
|
}; |
231 |
|
|
|
232 |
|
|
} // namespace crocoddyl |
233 |
|
|
|
234 |
|
|
/* --- Details -------------------------------------------------------------- */ |
235 |
|
|
/* --- Details -------------------------------------------------------------- */ |
236 |
|
|
/* --- Details -------------------------------------------------------------- */ |
237 |
|
|
#include "crocoddyl/core/numdiff/cost.hxx" |
238 |
|
|
|
239 |
|
|
#endif // CROCODDYL_CORE_NUMDIFF_COST_HPP_ |