GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh, CTU, INRIA, |
||
5 |
// University of Oxford, Heriot-Watt University |
||
6 |
// Copyright note valid unless otherwise stated in individual files. |
||
7 |
// All rights reserved. |
||
8 |
/////////////////////////////////////////////////////////////////////////////// |
||
9 |
|||
10 |
#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ |
||
11 |
#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ |
||
12 |
|||
13 |
#include <stdexcept> |
||
14 |
|||
15 |
#include "crocoddyl/core/actuation-base.hpp" |
||
16 |
#include "crocoddyl/core/constraints/constraint-manager.hpp" |
||
17 |
#include "crocoddyl/core/costs/cost-sum.hpp" |
||
18 |
#include "crocoddyl/core/diff-action-base.hpp" |
||
19 |
#include "crocoddyl/multibody/contacts/multiple-contacts.hpp" |
||
20 |
#include "crocoddyl/multibody/data/contacts.hpp" |
||
21 |
#include "crocoddyl/multibody/fwd.hpp" |
||
22 |
#include "crocoddyl/multibody/states/multibody.hpp" |
||
23 |
|||
24 |
namespace crocoddyl { |
||
25 |
|||
26 |
/** |
||
27 |
* @brief Differential action model for contact forward dynamics in multibody |
||
28 |
* systems. |
||
29 |
* |
||
30 |
* This class implements contact forward dynamics given a stack of |
||
31 |
* rigid-contacts described in `ContactModelMultipleTpl`, i.e., \f[ |
||
32 |
* \left[\begin{matrix}\dot{\mathbf{v}} |
||
33 |
* \\ -\boldsymbol{\lambda}\end{matrix}\right] = \left[\begin{matrix}\mathbf{M} |
||
34 |
* & \mathbf{J}^{\top}_c \\ {\mathbf{J}_{c}} & \mathbf{0} |
||
35 |
* \end{matrix}\right]^{-1} \left[\begin{matrix}\boldsymbol{\tau}_b |
||
36 |
* \\ -\mathbf{a}_0 \\\end{matrix}\right], \f] where \f$\mathbf{q}\in Q\f$, |
||
37 |
* \f$\mathbf{v}\in\mathbb{R}^{nv}\f$ are the configuration point and |
||
38 |
* generalized velocity (its tangent vector), respectively; |
||
39 |
* \f$\boldsymbol{\tau}_b=\boldsymbol{\tau} - |
||
40 |
* \mathbf{h}(\mathbf{q},\mathbf{v})\f$ is the bias forces that depends on the |
||
41 |
* torque inputs \f$\boldsymbol{\tau}\f$ and the Coriolis effect and gravity |
||
42 |
* field \f$\mathbf{h}(\mathbf{q},\mathbf{v})\f$; |
||
43 |
* \f$\mathbf{J}_c\in\mathbb{R}^{nc\times nv}\f$ is the contact Jacobian |
||
44 |
* expressed in the local frame; and \f$\mathbf{a}_0\in\mathbb{R}^{nc}\f$ is the |
||
45 |
* desired acceleration in the constraint space. To improve stability in the |
||
46 |
* numerical integration, we define PD gains that are similar in spirit to |
||
47 |
* Baumgarte stabilization: \f[ \mathbf{a}_0 = \mathbf{a}_{\lambda(c)} - \alpha |
||
48 |
* \,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)} |
||
49 |
* - \beta\mathbf{v}_{\lambda(c)}, \f] where \f$\mathbf{v}_{\lambda(c)}\f$, |
||
50 |
* \f$\mathbf{a}_{\lambda(c)}\f$ are the spatial velocity and acceleration at |
||
51 |
* the parent body of the contact \f$\lambda(c)\f$, respectively; \f$\alpha\f$ |
||
52 |
* and \f$\beta\f$ are the stabilization gains; |
||
53 |
* \f$\,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)}\f$ is the |
||
54 |
* \f$\mathbb{SE}(3)\f$ inverse composition between the reference contact |
||
55 |
* placement and the current one. |
||
56 |
* |
||
57 |
* The derivatives of the system acceleration and contact forces are computed |
||
58 |
* efficiently based on the analytical derivatives of Recursive Newton Euler |
||
59 |
* Algorithm (RNEA) as described in \cite mastalli-icra20. Note that the |
||
60 |
* algorithm for computing the RNEA derivatives is described in \cite |
||
61 |
* carpentier-rss18. |
||
62 |
* |
||
63 |
* The stack of cost and constraint functions are implemented in |
||
64 |
* `CostModelSumTpl` and `ConstraintModelAbstractTpl`, respectively. The |
||
65 |
* computation of the contact dynamics and its derivatives are carrying out |
||
66 |
* inside `calc()` and `calcDiff()` functions, respectively. It is also |
||
67 |
* important to remark that `calcDiff()` computes the derivatives using the |
||
68 |
* latest stored values by `calc()`. Thus, we need to run `calc()` first. |
||
69 |
* |
||
70 |
* \sa `DifferentialActionModelAbstractTpl`, `calc()`, `calcDiff()`, |
||
71 |
* `createData()` |
||
72 |
*/ |
||
73 |
template <typename _Scalar> |
||
74 |
class DifferentialActionModelContactFwdDynamicsTpl |
||
75 |
: public DifferentialActionModelAbstractTpl<_Scalar> { |
||
76 |
public: |
||
77 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
78 |
|||
79 |
typedef _Scalar Scalar; |
||
80 |
typedef DifferentialActionModelAbstractTpl<Scalar> Base; |
||
81 |
typedef DifferentialActionDataContactFwdDynamicsTpl<Scalar> Data; |
||
82 |
typedef MathBaseTpl<Scalar> MathBase; |
||
83 |
typedef StateMultibodyTpl<Scalar> StateMultibody; |
||
84 |
typedef CostModelSumTpl<Scalar> CostModelSum; |
||
85 |
typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager; |
||
86 |
typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple; |
||
87 |
typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract; |
||
88 |
typedef DifferentialActionDataAbstractTpl<Scalar> |
||
89 |
DifferentialActionDataAbstract; |
||
90 |
typedef typename MathBase::VectorXs VectorXs; |
||
91 |
typedef typename MathBase::MatrixXs MatrixXs; |
||
92 |
|||
93 |
/** |
||
94 |
* @brief Initialize the contact forward-dynamics action model |
||
95 |
* |
||
96 |
* It describes the dynamics evolution of a multibody system under |
||
97 |
* rigid-contact constraints defined by `ContactModelMultipleTpl`. It computes |
||
98 |
* the cost described in `CostModelSumTpl`. |
||
99 |
* |
||
100 |
* @param[in] state State of the multibody system |
||
101 |
* @param[in] actuation Actuation model |
||
102 |
* @param[in] contacts Stack of rigid contact |
||
103 |
* @param[in] costs Stack of cost functions |
||
104 |
* @param[in] JMinvJt_damping Damping term used in operational space inertia |
||
105 |
* matrix (default 0.) |
||
106 |
* @param[in] enable_force Enable the computation of the contact force |
||
107 |
* derivatives (default false) |
||
108 |
*/ |
||
109 |
DifferentialActionModelContactFwdDynamicsTpl( |
||
110 |
boost::shared_ptr<StateMultibody> state, |
||
111 |
boost::shared_ptr<ActuationModelAbstract> actuation, |
||
112 |
boost::shared_ptr<ContactModelMultiple> contacts, |
||
113 |
boost::shared_ptr<CostModelSum> costs, |
||
114 |
const Scalar JMinvJt_damping = Scalar(0.), |
||
115 |
const bool enable_force = false); |
||
116 |
|||
117 |
/** |
||
118 |
* @brief Initialize the contact forward-dynamics action model |
||
119 |
* |
||
120 |
* It describes the dynamics evolution of a multibody system under |
||
121 |
* rigid-contact constraints defined by `ContactModelMultipleTpl`. It computes |
||
122 |
* the cost described in `CostModelSumTpl`. |
||
123 |
* |
||
124 |
* @param[in] state State of the multibody system |
||
125 |
* @param[in] actuation Actuation model |
||
126 |
* @param[in] contacts Stack of rigid contact |
||
127 |
* @param[in] costs Stack of cost functions |
||
128 |
* @param[in] constraints Stack of constraints |
||
129 |
* @param[in] JMinvJt_damping Damping term used in operational space inertia |
||
130 |
* matrix (default 0.) |
||
131 |
* @param[in] enable_force Enable the computation of the contact force |
||
132 |
* derivatives (default false) |
||
133 |
*/ |
||
134 |
DifferentialActionModelContactFwdDynamicsTpl( |
||
135 |
boost::shared_ptr<StateMultibody> state, |
||
136 |
boost::shared_ptr<ActuationModelAbstract> actuation, |
||
137 |
boost::shared_ptr<ContactModelMultiple> contacts, |
||
138 |
boost::shared_ptr<CostModelSum> costs, |
||
139 |
boost::shared_ptr<ConstraintModelManager> constraints, |
||
140 |
const Scalar JMinvJt_damping = Scalar(0.), |
||
141 |
const bool enable_force = false); |
||
142 |
virtual ~DifferentialActionModelContactFwdDynamicsTpl(); |
||
143 |
|||
144 |
/** |
||
145 |
* @brief Compute the system acceleration, and cost value |
||
146 |
* |
||
147 |
* It computes the system acceleration using the contact dynamics. |
||
148 |
* |
||
149 |
* @param[in] data Contact forward-dynamics data |
||
150 |
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
||
151 |
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ |
||
152 |
*/ |
||
153 |
virtual void calc( |
||
154 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
155 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u); |
||
156 |
|||
157 |
/** |
||
158 |
* @brief Compute the total cost value for nodes that depends only on the |
||
159 |
* state |
||
160 |
* |
||
161 |
* It updates the total cost and the system acceleration is not updated as it |
||
162 |
* is expected to be zero. Additionally, it does not update the contact |
||
163 |
* forces. This function is used in the terminal nodes of an optimal control |
||
164 |
* problem. |
||
165 |
* |
||
166 |
* @param[in] data Contact forward-dynamics data |
||
167 |
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
||
168 |
*/ |
||
169 |
virtual void calc( |
||
170 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
171 |
const Eigen::Ref<const VectorXs>& x); |
||
172 |
|||
173 |
/** |
||
174 |
* @brief Compute the derivatives of the contact dynamics, and cost function |
||
175 |
* |
||
176 |
* @param[in] data Contact forward-dynamics data |
||
177 |
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
||
178 |
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ |
||
179 |
*/ |
||
180 |
virtual void calcDiff( |
||
181 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
182 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u); |
||
183 |
|||
184 |
/** |
||
185 |
* @brief Compute the derivatives of the cost functions with respect to the |
||
186 |
* state only |
||
187 |
* |
||
188 |
* It updates the derivatives of the cost function with respect to the state |
||
189 |
* only. Additionally, it does not update the contact forces derivatives. This |
||
190 |
* function is used in the terminal nodes of an optimal control problem. |
||
191 |
* |
||
192 |
* @param[in] data Contact forward-dynamics data |
||
193 |
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
||
194 |
*/ |
||
195 |
virtual void calcDiff( |
||
196 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
197 |
const Eigen::Ref<const VectorXs>& x); |
||
198 |
|||
199 |
/** |
||
200 |
* @brief Create the contact forward-dynamics data |
||
201 |
* |
||
202 |
* @return contact forward-dynamics data |
||
203 |
*/ |
||
204 |
virtual boost::shared_ptr<DifferentialActionDataAbstract> createData(); |
||
205 |
|||
206 |
/** |
||
207 |
* @brief Check that the given data belongs to the contact forward-dynamics |
||
208 |
* data |
||
209 |
*/ |
||
210 |
virtual bool checkData( |
||
211 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data); |
||
212 |
|||
213 |
/** |
||
214 |
* @brief @copydoc Base::quasiStatic() |
||
215 |
*/ |
||
216 |
virtual void quasiStatic( |
||
217 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
218 |
Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, |
||
219 |
const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9)); |
||
220 |
|||
221 |
/** |
||
222 |
* @brief Return the number of inequality constraints |
||
223 |
*/ |
||
224 |
virtual std::size_t get_ng() const; |
||
225 |
|||
226 |
/** |
||
227 |
* @brief Return the number of equality constraints |
||
228 |
*/ |
||
229 |
virtual std::size_t get_nh() const; |
||
230 |
|||
231 |
/** |
||
232 |
* @brief Return the lower bound of the inequality constraints |
||
233 |
*/ |
||
234 |
virtual const VectorXs& get_g_lb() const; |
||
235 |
|||
236 |
/** |
||
237 |
* @brief Return the upper bound of the inequality constraints |
||
238 |
*/ |
||
239 |
virtual const VectorXs& get_g_ub() const; |
||
240 |
|||
241 |
/** |
||
242 |
* @brief Return the actuation model |
||
243 |
*/ |
||
244 |
const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const; |
||
245 |
|||
246 |
/** |
||
247 |
* @brief Return the contact model |
||
248 |
*/ |
||
249 |
const boost::shared_ptr<ContactModelMultiple>& get_contacts() const; |
||
250 |
|||
251 |
/** |
||
252 |
* @brief Return the cost model |
||
253 |
*/ |
||
254 |
const boost::shared_ptr<CostModelSum>& get_costs() const; |
||
255 |
|||
256 |
/** |
||
257 |
* @brief Return the constraint model manager |
||
258 |
*/ |
||
259 |
const boost::shared_ptr<ConstraintModelManager>& get_constraints() const; |
||
260 |
|||
261 |
/** |
||
262 |
* @brief Return the Pinocchio model |
||
263 |
*/ |
||
264 |
pinocchio::ModelTpl<Scalar>& get_pinocchio() const; |
||
265 |
|||
266 |
/** |
||
267 |
* @brief Return the armature vector |
||
268 |
*/ |
||
269 |
const VectorXs& get_armature() const; |
||
270 |
|||
271 |
/** |
||
272 |
* @brief Return the damping factor used in operational space inertia matrix |
||
273 |
*/ |
||
274 |
const Scalar get_damping_factor() const; |
||
275 |
|||
276 |
/** |
||
277 |
* @brief Modify the armature vector |
||
278 |
*/ |
||
279 |
void set_armature(const VectorXs& armature); |
||
280 |
|||
281 |
/** |
||
282 |
* @brief Modify the damping factor used in operational space inertia matrix |
||
283 |
*/ |
||
284 |
void set_damping_factor(const Scalar damping); |
||
285 |
|||
286 |
/** |
||
287 |
* @brief Print relevant information of the contact forward-dynamics model |
||
288 |
* |
||
289 |
* @param[out] os Output stream object |
||
290 |
*/ |
||
291 |
virtual void print(std::ostream& os) const; |
||
292 |
|||
293 |
protected: |
||
294 |
using Base::g_lb_; //!< Lower bound of the inequality constraints |
||
295 |
using Base::g_ub_; //!< Upper bound of the inequality constraints |
||
296 |
using Base::nu_; //!< Control dimension |
||
297 |
using Base::state_; //!< Model of the state |
||
298 |
|||
299 |
private: |
||
300 |
void init(); |
||
301 |
boost::shared_ptr<ActuationModelAbstract> actuation_; //!< Actuation model |
||
302 |
boost::shared_ptr<ContactModelMultiple> contacts_; //!< Contact model |
||
303 |
boost::shared_ptr<CostModelSum> costs_; //!< Cost model |
||
304 |
boost::shared_ptr<ConstraintModelManager> constraints_; //!< Constraint model |
||
305 |
pinocchio::ModelTpl<Scalar>& pinocchio_; //!< Pinocchio model |
||
306 |
bool with_armature_; //!< Indicate if we have defined an armature |
||
307 |
VectorXs armature_; //!< Armature vector |
||
308 |
Scalar JMinvJt_damping_; //!< Damping factor used in operational space |
||
309 |
//!< inertia matrix |
||
310 |
bool enable_force_; //!< Indicate if we have enabled the computation of the |
||
311 |
//!< contact-forces derivatives |
||
312 |
}; |
||
313 |
|||
314 |
template <typename _Scalar> |
||
315 |
struct DifferentialActionDataContactFwdDynamicsTpl |
||
316 |
: public DifferentialActionDataAbstractTpl<_Scalar> { |
||
317 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
318 |
typedef _Scalar Scalar; |
||
319 |
typedef MathBaseTpl<Scalar> MathBase; |
||
320 |
typedef DifferentialActionDataAbstractTpl<Scalar> Base; |
||
321 |
typedef JointDataAbstractTpl<Scalar> JointDataAbstract; |
||
322 |
typedef DataCollectorJointActMultibodyInContactTpl<Scalar> |
||
323 |
DataCollectorJointActMultibodyInContact; |
||
324 |
typedef typename MathBase::VectorXs VectorXs; |
||
325 |
typedef typename MathBase::MatrixXs MatrixXs; |
||
326 |
|||
327 |
template <template <typename Scalar> class Model> |
||
328 |
39554 |
explicit DifferentialActionDataContactFwdDynamicsTpl( |
|
329 |
Model<Scalar>* const model) |
||
330 |
: Base(model), |
||
331 |
39554 |
pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())), |
|
332 |
multibody( |
||
333 |
39554 |
&pinocchio, model->get_actuation()->createData(), |
|
334 |
39554 |
boost::make_shared<JointDataAbstract>( |
|
335 |
model->get_state(), model->get_actuation(), model->get_nu()), |
||
336 |
39554 |
model->get_contacts()->createData(&pinocchio)), |
|
337 |
39554 |
costs(model->get_costs()->createData(&multibody)), |
|
338 |
79108 |
Kinv(model->get_state()->get_nv() + |
|
339 |
39554 |
model->get_contacts()->get_nc_total(), |
|
340 |
79108 |
model->get_state()->get_nv() + |
|
341 |
39554 |
model->get_contacts()->get_nc_total()), |
|
342 |
39554 |
df_dx(model->get_contacts()->get_nc_total(), |
|
343 |
39554 |
model->get_state()->get_ndx()), |
|
344 |
39554 |
df_du(model->get_contacts()->get_nc_total(), model->get_nu()), |
|
345 |
39554 |
tmp_xstatic(model->get_state()->get_nx()), |
|
346 |
39554 |
tmp_Jstatic(model->get_state()->get_nv(), |
|
347 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
158216 |
model->get_nu() + model->get_contacts()->get_nc_total()) { |
348 |
✓✗✓✗ |
39554 |
multibody.joint->dtau_du.diagonal().setOnes(); |
349 |
✓✗ | 39554 |
costs->shareMemory(this); |
350 |
✓✓ | 39554 |
if (model->get_constraints() != nullptr) { |
351 |
✓✗ | 1160 |
constraints = model->get_constraints()->createData(&multibody); |
352 |
✓✗ | 1160 |
constraints->shareMemory(this); |
353 |
} |
||
354 |
✓✗ | 39554 |
Kinv.setZero(); |
355 |
✓✗ | 39554 |
df_dx.setZero(); |
356 |
✓✗ | 39554 |
df_du.setZero(); |
357 |
✓✗ | 39554 |
tmp_xstatic.setZero(); |
358 |
✓✗ | 39554 |
tmp_Jstatic.setZero(); |
359 |
✓✗ | 39554 |
pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total()); |
360 |
✓✗ | 39554 |
pinocchio.lambda_c.setZero(); |
361 |
39554 |
} |
|
362 |
|||
363 |
pinocchio::DataTpl<Scalar> pinocchio; |
||
364 |
DataCollectorJointActMultibodyInContact multibody; |
||
365 |
boost::shared_ptr<CostDataSumTpl<Scalar> > costs; |
||
366 |
boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints; |
||
367 |
MatrixXs Kinv; |
||
368 |
MatrixXs df_dx; |
||
369 |
MatrixXs df_du; |
||
370 |
VectorXs tmp_xstatic; |
||
371 |
MatrixXs tmp_Jstatic; |
||
372 |
|||
373 |
using Base::cost; |
||
374 |
using Base::Fu; |
||
375 |
using Base::Fx; |
||
376 |
using Base::Lu; |
||
377 |
using Base::Luu; |
||
378 |
using Base::Lx; |
||
379 |
using Base::Lxu; |
||
380 |
using Base::Lxx; |
||
381 |
using Base::r; |
||
382 |
using Base::xout; |
||
383 |
}; |
||
384 |
|||
385 |
} // namespace crocoddyl |
||
386 |
|||
387 |
/* --- Details -------------------------------------------------------------- */ |
||
388 |
/* --- Details -------------------------------------------------------------- */ |
||
389 |
/* --- Details -------------------------------------------------------------- */ |
||
390 |
#include <crocoddyl/multibody/actions/contact-fwddyn.hxx> |
||
391 |
|||
392 |
#endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ |
Generated by: GCOVR (Version 4.2) |