Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/multiple-contacts.hpp	Lines:	21	24	87.5 %
Date:	2024-02-13 11:12:33	Branches:	17	32	53.1 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
#define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_

#include <map>
#include <set>
#include <string>
#include <utility>

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/contact-base.hpp"
#include "crocoddyl/multibody/fwd.hpp"

namespace crocoddyl {

template <typename _Scalar>
struct ContactItemTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;

  ContactItemTpl() {}
  ContactItemTpl(const std::string& name,
                 boost::shared_ptr<ContactModelAbstract> contact,
                 const bool active = true)
      : name(name), contact(contact), active(active) {}

  /**
   * @brief Print information on the contact item
   */
  friend std::ostream& operator<<(std::ostream& os,
                                  const ContactItemTpl<Scalar>& model) {
    os << "{" << *model.contact << "}";
    return os;
  }

  std::string name;
  boost::shared_ptr<ContactModelAbstract> contact;
  bool active;
};

/**
 * @brief Define a stack of contact models
 *
 * The contact models can be defined with active and inactive status. The idea
 * behind this design choice is to be able to create a mechanism that allocates
 * the entire data needed for the computations. Then, there are designed
 * routines that update the only active contacts.
 */
template <typename _Scalar>
class ContactModelMultipleTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef StateMultibodyTpl<Scalar> StateMultibody;
  typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
  typedef ContactDataMultipleTpl<Scalar> ContactDataMultiple;
  typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;

  typedef ContactItemTpl<Scalar> ContactItem;

  typedef typename MathBase::Vector2s Vector2s;
  typedef typename MathBase::Vector3s Vector3s;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  typedef std::map<std::string, boost::shared_ptr<ContactItem> >
      ContactModelContainer;
  typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> >
      ContactDataContainer;
  typedef typename pinocchio::container::aligned_vector<
      pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;

  /**
   * @brief Initialize the multi-contact model
   *
   * @param[in] state  Multibody state
   * @param[in] nu     Dimension of control vector
   */
  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
                          const std::size_t nu);

  /**
   * @brief Initialize the multi-contact model
   *
   * @param[in] state  Multibody state
   */
  ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
  ~ContactModelMultipleTpl();

  /**
   * @brief Add contact item
   *
   * Note that the memory is allocated for inactive contacts as well.
   *
   * @param[in] name     Contact name
   * @param[in] contact  Contact model
   * @param[in] active   Contact status (active by default)
   */
  void addContact(const std::string& name,
                  boost::shared_ptr<ContactModelAbstract> contact,
                  const bool active = true);

  /**
   * @brief Remove contact item
   *
   * @param[in] name  Contact name
   */
  void removeContact(const std::string& name);

  /**
   * @brief Change the contact status
   *
   * @param[in] name     Contact name
   * @param[in] active   Contact status (True for active)
   */
  void changeContactStatus(const std::string& name, const bool active);

  /**
   * @brief Compute the contact Jacobian and contact acceleration
   *
   * @param[in] data  Multi-contact data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calc(const boost::shared_ptr<ContactDataMultiple>& data,
            const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Compute the derivatives of the contact holonomic constraint
   *
   * @param[in] data  Multi-contact data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   */
  void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data,
                const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Update the constrained system acceleration
   *
   * @param[in] data  Multi-contact data
   * @param[in] dv    Constrained system acceleration
   * \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
   */
  void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data,
                          const VectorXs& dv) const;

  /**
   * @brief Update the spatial force defined in frame coordinate
   *
   * @param[in] data   Multi-contact data
   * @param[in] force  Spatial force defined in frame coordinate
   * \f${}^o\underline{\boldsymbol{\lambda}}_c\in\mathbb{R}^{nc}\f$
   */
  void updateForce(const boost::shared_ptr<ContactDataMultiple>& data,
                   const VectorXs& force);

  /**
   * @brief Update the Jacobian of the constrained system acceleration
   *
   * @param[in] data    Multi-contact data
   * @param[in] ddv_dx  Jacobian of the system acceleration in generalized
   * coordinates
   * \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
   * ndx}\f$
   */
  void updateAccelerationDiff(
      const boost::shared_ptr<ContactDataMultiple>& data,
      const MatrixXs& ddv_dx) const;

  /**
   * @brief Update the Jacobian of the spatial force defined in frame coordinate
   *
   * @param[in] data   Multi-contact data
   * @param[in] df_dx  Jacobian of the spatial impulse defined in frame
   * coordinate
   * \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{x}}\in\mathbb{R}^{nc\times{ndx}}\f$
   * @param[in] df_du  Jacobian of the spatial impulse defined in frame
   * coordinate
   * \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{u}}\in\mathbb{R}^{nc\times{nu}}\f$
   */
  void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
                       const MatrixXs& df_dx, const MatrixXs& df_du) const;

  /**
   * @brief Update the RNEA derivatives dtau_dq by adding the skew term
   * (necessary for contacts expressed in LOCAL_WORLD_ALIGNED / WORLD)
   *
   * To learn more about the computation of the contact derivatives in different
   * frames see https://hal.science/hal-03758989/document.
   *
   * @param[in] data       Multi-contact data
   * @param[in] pinocchio  Pinocchio data
   */
  void updateRneaDiff(const boost::shared_ptr<ContactDataMultiple>& data,
                      pinocchio::DataTpl<Scalar>& pinocchio) const;

  /**
   * @brief Create the multi-contact data
   *
   * @param[in] data  Pinocchio data
   * @return the multi-contact data.
   */
  boost::shared_ptr<ContactDataMultiple> createData(
      pinocchio::DataTpl<Scalar>* const data);

  /**
   * @brief Return the multibody state
   */
  const boost::shared_ptr<StateMultibody>& get_state() const;

  /**
   * @brief Return the contact models
   */
  const ContactModelContainer& get_contacts() const;

  /**
   * @brief Return the dimension of active contacts
   */
  std::size_t get_nc() const;

  /**
   * @brief Return the dimension of all contacts
   */
  std::size_t get_nc_total() const;

  /**
   * @brief Return the dimension of control vector
   */
  std::size_t get_nu() const;

  /**
   * @brief Return the names of the set of active contacts
   */
  const std::set<std::string>& get_active_set() const;

  /**
   * @brief Return the names of the set of inactive contacts
   */
  const std::set<std::string>& get_inactive_set() const;

  /**
   * @brief Return the status of a given contact name
   */
  bool getContactStatus(const std::string& name) const;

  /**
   * @brief Return the type of contact computation
   *
   * True for all contacts, otherwise false for active contacts
   */
  bool getComputeAllContacts() const;

  /**
   * @brief Set the tyoe of contact computation: all or active contacts
   *
   * @param status  Type of contact computation (true for all contacts and false
   * for active contacts)
   */
  void setComputeAllContacts(const bool status);

  /**
   * @brief Print information on the contact models
   */
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os,
                                  const ContactModelMultipleTpl<Scalar>& model);

 private:
  boost::shared_ptr<StateMultibody> state_;
  ContactModelContainer contacts_;
  std::size_t nc_;
  std::size_t nc_total_;
  std::size_t nu_;
  std::set<std::string> active_set_;
  std::set<std::string> inactive_set_;
  bool compute_all_contacts_;
};

/**
 * @brief Define the multi-contact data
 *
 * \sa ContactModelMultipleTpl
 */
template <typename _Scalar>
struct ContactDataMultipleTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
  typedef ContactItemTpl<Scalar> ContactItem;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialized a multi-contact data
   *
   * @param[in] model  Multi-contact model
   * @param[in] data   Pinocchio data
   */
  template <template <typename Scalar> class Model>
  ContactDataMultipleTpl(Model<Scalar>* const model,
                         pinocchio::DataTpl<Scalar>* const data)
      : Jc(model->get_nc_total(), model->get_state()->get_nv()),
        a0(model->get_nc_total()),
        da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
        dv(model->get_state()->get_nv()),
        ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        fext(model->get_state()->get_pinocchio()->njoints,
             pinocchio::ForceTpl<Scalar>::Zero()) {
    Jc.setZero();
    a0.setZero();
    da0_dx.setZero();
    dv.setZero();
    ddv_dx.setZero();
    for (typename ContactModelMultiple::ContactModelContainer::const_iterator
             it = model->get_contacts().begin();
         it != model->get_contacts().end(); ++it) {
      const boost::shared_ptr<ContactItem>& item = it->second;
      contacts.insert(
          std::make_pair(item->name, item->contact->createData(data)));
    }
  }

  MatrixXs Jc;  //!< Contact Jacobian in frame coordinate
                //!< \f$\mathbf{J}_c\in\mathbb{R}^{nc_{total}\times{nv}}\f$
                //!< (memory defined for active and inactive contacts)
  VectorXs a0;  //!< Desired spatial contact acceleration in frame coordinate
                //!< \f$\underline{\mathbf{a}}_0\in\mathbb{R}^{nc_{total}}\f$
                //!< (memory defined for active and inactive contacts)
  MatrixXs
      da0_dx;  //!< Jacobian of the desired spatial contact acceleration in
               //!< frame coordinate
               //!< \f$\frac{\partial\underline{\mathbf{a}}_0}{\partial\mathbf{x}}\in\mathbb{R}^{nc_{total}\times{ndx}}\f$
               //!< (memory defined for active and inactive contacts)
  VectorXs dv;  //!< Constrained system acceleration in generalized coordinates
                //!< \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
  MatrixXs
      ddv_dx;  //!< Jacobian of the system acceleration in generalized
               //!< coordinates
               //!< \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
               //!< ndx}\f$
  typename ContactModelMultiple::ContactDataContainer
      contacts;  //!< Stack of contact data
  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
      fext;  //!< External spatial forces in body coordinates
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/multibody/contacts/multiple-contacts.hxx"

#endif  // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
12
13			#include <map>
14			#include <set>
15			#include <string>
16			#include <utility>
17
18			#include "crocoddyl/core/utils/exception.hpp"
19			#include "crocoddyl/multibody/contact-base.hpp"
20			#include "crocoddyl/multibody/fwd.hpp"
21
22			namespace crocoddyl {
23
24			template <typename _Scalar>
25			struct ContactItemTpl {
26			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28			typedef _Scalar Scalar;
29			typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;
30
31			ContactItemTpl() {}
32		1717	ContactItemTpl(const std::string& name,
33			boost::shared_ptr<ContactModelAbstract> contact,
34			const bool active = true)
35		1717	: name(name), contact(contact), active(active) {}
36
37			/**
38			* @brief Print information on the contact item
39			*/
40			friend std::ostream& operator<<(std::ostream& os,
41			const ContactItemTpl<Scalar>& model) {
42			os << "{" << *model.contact << "}";
43			return os;
44			}
45
46			std::string name;
47			boost::shared_ptr<ContactModelAbstract> contact;
48			bool active;
49			};
50
51			/**
52			* @brief Define a stack of contact models
53			*
54			* The contact models can be defined with active and inactive status. The idea
55			* behind this design choice is to be able to create a mechanism that allocates
56			* the entire data needed for the computations. Then, there are designed
57			* routines that update the only active contacts.
58			*/
59			template <typename _Scalar>
60			class ContactModelMultipleTpl {
61			public:
62			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63
64			typedef _Scalar Scalar;
65			typedef MathBaseTpl<Scalar> MathBase;
66			typedef StateMultibodyTpl<Scalar> StateMultibody;
67			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
68			typedef ContactDataMultipleTpl<Scalar> ContactDataMultiple;
69			typedef ContactModelAbstractTpl<Scalar> ContactModelAbstract;
70
71			typedef ContactItemTpl<Scalar> ContactItem;
72
73			typedef typename MathBase::Vector2s Vector2s;
74			typedef typename MathBase::Vector3s Vector3s;
75			typedef typename MathBase::VectorXs VectorXs;
76			typedef typename MathBase::MatrixXs MatrixXs;
77
78			typedef std::map<std::string, boost::shared_ptr<ContactItem> >
79			ContactModelContainer;
80			typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> >
81			ContactDataContainer;
82			typedef typename pinocchio::container::aligned_vector<
83			pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
84
85			/**
86			* @brief Initialize the multi-contact model
87			*
88			* @param[in] state Multibody state
89			* @param[in] nu Dimension of control vector
90			*/
91			ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
92			const std::size_t nu);
93
94			/**
95			* @brief Initialize the multi-contact model
96			*
97			* @param[in] state Multibody state
98			*/
99			ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
100			~ContactModelMultipleTpl();
101
102			/**
103			* @brief Add contact item
104			*
105			* Note that the memory is allocated for inactive contacts as well.
106			*
107			* @param[in] name Contact name
108			* @param[in] contact Contact model
109			* @param[in] active Contact status (active by default)
110			*/
111			void addContact(const std::string& name,
112			boost::shared_ptr<ContactModelAbstract> contact,
113			const bool active = true);
114
115			/**
116			* @brief Remove contact item
117			*
118			* @param[in] name Contact name
119			*/
120			void removeContact(const std::string& name);
121
122			/**
123			* @brief Change the contact status
124			*
125			* @param[in] name Contact name
126			* @param[in] active Contact status (True for active)
127			*/
128			void changeContactStatus(const std::string& name, const bool active);
129
130			/**
131			* @brief Compute the contact Jacobian and contact acceleration
132			*
133			* @param[in] data Multi-contact data
134			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
135			*/
136			void calc(const boost::shared_ptr<ContactDataMultiple>& data,
137			const Eigen::Ref<const VectorXs>& x);
138
139			/**
140			* @brief Compute the derivatives of the contact holonomic constraint
141			*
142			* @param[in] data Multi-contact data
143			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
144			*/
145			void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data,
146			const Eigen::Ref<const VectorXs>& x);
147
148			/**
149			* @brief Update the constrained system acceleration
150			*
151			* @param[in] data Multi-contact data
152			* @param[in] dv Constrained system acceleration
153			* \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
154			*/
155			void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data,
156			const VectorXs& dv) const;
157
158			/**
159			* @brief Update the spatial force defined in frame coordinate
160			*
161			* @param[in] data Multi-contact data
162			* @param[in] force Spatial force defined in frame coordinate
163			* \f${}^o\underline{\boldsymbol{\lambda}}_c\in\mathbb{R}^{nc}\f$
164			*/
165			void updateForce(const boost::shared_ptr<ContactDataMultiple>& data,
166			const VectorXs& force);
167
168			/**
169			* @brief Update the Jacobian of the constrained system acceleration
170			*
171			* @param[in] data Multi-contact data
172			* @param[in] ddv_dx Jacobian of the system acceleration in generalized
173			* coordinates
174			* \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
175			* ndx}\f$
176			*/
177			void updateAccelerationDiff(
178			const boost::shared_ptr<ContactDataMultiple>& data,
179			const MatrixXs& ddv_dx) const;
180
181			/**
182			* @brief Update the Jacobian of the spatial force defined in frame coordinate
183			*
184			* @param[in] data Multi-contact data
185			* @param[in] df_dx Jacobian of the spatial impulse defined in frame
186			* coordinate
187			* \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{x}}\in\mathbb{R}^{nc\times{ndx}}\f$
188			* @param[in] df_du Jacobian of the spatial impulse defined in frame
189			* coordinate
190			* \f$\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{u}}\in\mathbb{R}^{nc\times{nu}}\f$
191			*/
192			void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
193			const MatrixXs& df_dx, const MatrixXs& df_du) const;
194
195			/**
196			* @brief Update the RNEA derivatives dtau_dq by adding the skew term
197			* (necessary for contacts expressed in LOCAL_WORLD_ALIGNED / WORLD)
198			*
199			* To learn more about the computation of the contact derivatives in different
200			* frames see https://hal.science/hal-03758989/document.
201			*
202			* @param[in] data Multi-contact data
203			* @param[in] pinocchio Pinocchio data
204			*/
205			void updateRneaDiff(const boost::shared_ptr<ContactDataMultiple>& data,
206			pinocchio::DataTpl<Scalar>& pinocchio) const;
207
208			/**
209			* @brief Create the multi-contact data
210			*
211			* @param[in] data Pinocchio data
212			* @return the multi-contact data.
213			*/
214			boost::shared_ptr<ContactDataMultiple> createData(
215			pinocchio::DataTpl<Scalar>* const data);
216
217			/**
218			* @brief Return the multibody state
219			*/
220			const boost::shared_ptr<StateMultibody>& get_state() const;
221
222			/**
223			* @brief Return the contact models
224			*/
225			const ContactModelContainer& get_contacts() const;
226
227			/**
228			* @brief Return the dimension of active contacts
229			*/
230			std::size_t get_nc() const;
231
232			/**
233			* @brief Return the dimension of all contacts
234			*/
235			std::size_t get_nc_total() const;
236
237			/**
238			* @brief Return the dimension of control vector
239			*/
240			std::size_t get_nu() const;
241
242			/**
243			* @brief Return the names of the set of active contacts
244			*/
245			const std::set<std::string>& get_active_set() const;
246
247			/**
248			* @brief Return the names of the set of inactive contacts
249			*/
250			const std::set<std::string>& get_inactive_set() const;
251
252			/**
253			* @brief Return the status of a given contact name
254			*/
255			bool getContactStatus(const std::string& name) const;
256
257			/**
258			* @brief Return the type of contact computation
259			*
260			* True for all contacts, otherwise false for active contacts
261			*/
262			bool getComputeAllContacts() const;
263
264			/**
265			* @brief Set the tyoe of contact computation: all or active contacts
266			*
267			* @param status Type of contact computation (true for all contacts and false
268			* for active contacts)
269			*/
270			void setComputeAllContacts(const bool status);
271
272			/**
273			* @brief Print information on the contact models
274			*/
275			template <class Scalar>
276			friend std::ostream& operator<<(std::ostream& os,
277			const ContactModelMultipleTpl<Scalar>& model);
278
279			private:
280			boost::shared_ptr<StateMultibody> state_;
281			ContactModelContainer contacts_;
282			std::size_t nc_;
283			std::size_t nc_total_;
284			std::size_t nu_;
285			std::set<std::string> active_set_;
286			std::set<std::string> inactive_set_;
287			bool compute_all_contacts_;
288			};
289
290			/**
291			* @brief Define the multi-contact data
292			*
293			* \sa ContactModelMultipleTpl
294			*/
295			template <typename _Scalar>
296			struct ContactDataMultipleTpl {
297			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
298
299			typedef _Scalar Scalar;
300			typedef MathBaseTpl<Scalar> MathBase;
301			typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
302			typedef ContactItemTpl<Scalar> ContactItem;
303			typedef typename MathBase::VectorXs VectorXs;
304			typedef typename MathBase::MatrixXs MatrixXs;
305
306			/**
307			* @brief Initialized a multi-contact data
308			*
309			* @param[in] model Multi-contact model
310			* @param[in] data Pinocchio data
311			*/
312			template <template <typename Scalar> class Model>
313		70999	ContactDataMultipleTpl(Model<Scalar>* const model,
314			pinocchio::DataTpl<Scalar>* const data)
315		70999	: Jc(model->get_nc_total(), model->get_state()->get_nv()),
316			a0(model->get_nc_total()),
317		70999	da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
318		70999	dv(model->get_state()->get_nv()),
319		141998	ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
320		70999	fext(model->get_state()->get_pinocchio()->njoints,
321	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	496993	pinocchio::ForceTpl<Scalar>::Zero()) {
322	✓✗	70999	Jc.setZero();
323	✓✗	70999	a0.setZero();
324	✓✗	70999	da0_dx.setZero();
325	✓✗	70999	dv.setZero();
326	✓✗	70999	ddv_dx.setZero();
327		160814	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
328		70999	it = model->get_contacts().begin();
329	✓✓	231813	it != model->get_contacts().end(); ++it) {
330		160814	const boost::shared_ptr<ContactItem>& item = it->second;
331	✓✗	160814	contacts.insert(
332	✓✗✓✗	160814	std::make_pair(item->name, item->contact->createData(data)));
333			}
334		70999	}
335
336			MatrixXs Jc; //!< Contact Jacobian in frame coordinate
337			//!< \f$\mathbf{J}_c\in\mathbb{R}^{nc_{total}\times{nv}}\f$
338			//!< (memory defined for active and inactive contacts)
339			VectorXs a0; //!< Desired spatial contact acceleration in frame coordinate
340			//!< \f$\underline{\mathbf{a}}_0\in\mathbb{R}^{nc_{total}}\f$
341			//!< (memory defined for active and inactive contacts)
342			MatrixXs
343			da0_dx; //!< Jacobian of the desired spatial contact acceleration in
344			//!< frame coordinate
345			//!< \f$\frac{\partial\underline{\mathbf{a}}_0}{\partial\mathbf{x}}\in\mathbb{R}^{nc_{total}\times{ndx}}\f$
346			//!< (memory defined for active and inactive contacts)
347			VectorXs dv; //!< Constrained system acceleration in generalized coordinates
348			//!< \f$\dot{\mathbf{v}}\in\mathbb{R}^{nv}\f$
349			MatrixXs
350			ddv_dx; //!< Jacobian of the system acceleration in generalized
351			//!< coordinates
352			//!< \f$\frac{\partial\dot{\mathbf{v}}}{\partial\mathbf{x}}\in\mathbb{R}^{nv\times
353			//!< ndx}\f$
354			typename ContactModelMultiple::ContactDataContainer
355			contacts; //!< Stack of contact data
356			pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
357			fext; //!< External spatial forces in body coordinates
358			};
359
360			} // namespace crocoddyl
361
362			/* --- Details -------------------------------------------------------------- */
363			/* --- Details -------------------------------------------------------------- */
364			/* --- Details -------------------------------------------------------------- */
365			#include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
366
367			#endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_