Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/contact-6d.hpp	Lines:	26	28	92.9 %
Date:	2024-02-13 11:12:33	Branches:	36	72	50.0 %


///////////////////////////////////////////////////////////////////////////////
// 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_CONTACT_6D_HPP_
#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_

#include <pinocchio/multibody/data.hpp>
#include <pinocchio/spatial/motion.hpp>

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

namespace crocoddyl {

template <typename _Scalar>
class ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ContactModelAbstractTpl<Scalar> Base;
  typedef ContactData6DTpl<Scalar> Data;
  typedef StateMultibodyTpl<Scalar> StateMultibody;
  typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
  typedef pinocchio::SE3Tpl<Scalar> SE3;
  typedef typename MathBase::Vector2s Vector2s;
  typedef typename MathBase::Vector3s Vector3s;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::Matrix3s Matrix3s;

  /**
   * @brief Initialize the 6d contact model
   *
   * To learn more about the computation of the contact derivatives in different
   * frames see
   *  S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
   *  Frames: Application to Polishing with Talos, ICHR 2022
   *
   * @param[in] state  State of the multibody system
   * @param[in] id     Reference frame id of the contact
   * @param[in] pref   Contact placement used for the Baumgarte stabilization
   * @param[in] type   Type of contact
   * @param[in] nu     Dimension of the control vector
   * @param[in] gains  Baumgarte stabilization gains
   */
  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
                    const pinocchio::FrameIndex id, const SE3& pref,
                    const pinocchio::ReferenceFrame type, const std::size_t nu,
                    const Vector2s& gains = Vector2s::Zero());

  /**
   * @brief Initialize the 6d contact model
   *
   * The default `nu` is obtained from `StateAbstractTpl::get_nv()`. To learn
   * more about the computation of the contact derivatives in different frames
   * see
   *  S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
   *  Frames: Application to Polishing with Talos, ICHR 2022
   *
   * @param[in] state  State of the multibody system
   * @param[in] id     Reference frame id of the contact
   * @param[in] pref   Contact placement used for the Baumgarte stabilization
   * @param[in] type   Type of contact
   * @param[in] gains  Baumgarte stabilization gains
   */
  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
                    const pinocchio::FrameIndex id, const SE3& pref,
                    const pinocchio::ReferenceFrame type,
                    const Vector2s& gains = Vector2s::Zero());

  DEPRECATED(
      "Use constructor that passes the type type of contact, this assumes is "
      "pinocchio::LOCAL",
      ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
                        const pinocchio::FrameIndex id, const SE3& pref,
                        const std::size_t nu,
                        const Vector2s& gains = Vector2s::Zero());)
  DEPRECATED(
      "Use constructor that passes the type type of contact, this assumes is "
      "pinocchio::LOCAL",
      ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
                        const pinocchio::FrameIndex id, const SE3& pref,
                        const Vector2s& gains = Vector2s::Zero());)
  virtual ~ContactModel6DTpl();

  /**
   * @brief Compute the 3d contact Jacobian and drift
   *
   * @param[in] data  3d contact data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x);

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

  /**
   * @brief Convert the force into a stack of spatial forces
   *
   * @param[in] data   6d contact data
   * @param[in] force  6d force
   */
  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data,
                           const VectorXs& force);

  /**
   * @brief Create the 6d contact data
   */
  virtual boost::shared_ptr<ContactDataAbstract> createData(
      pinocchio::DataTpl<Scalar>* const data);

  /**
   * @brief Return the reference frame placement
   */
  const SE3& get_reference() const;

  /**
   * @brief Return the Baumgarte stabilization gains
   */
  const Vector2s& get_gains() const;

  /**
   * @brief Modify the reference frame placement
   */
  void set_reference(const SE3& reference);

  /**
   * @brief Print relevant information of the 6d contact model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream& os) const;

 protected:
  using Base::id_;
  using Base::nc_;
  using Base::nu_;
  using Base::state_;
  using Base::type_;

 private:
  SE3 pref_;        //!< Contact placement used for the Baumgarte stabilization
  Vector2s gains_;  //!< Baumgarte stabilization gains
};

template <typename _Scalar>
struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ContactDataAbstractTpl<Scalar> Base;
  typedef typename MathBase::Matrix3s Matrix3s;
  typedef typename MathBase::Matrix6xs Matrix6xs;
  typedef typename MathBase::Matrix6s Matrix6s;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef typename pinocchio::SE3Tpl<Scalar> SE3;
  typedef typename pinocchio::MotionTpl<Scalar> Motion;
  typedef typename pinocchio::ForceTpl<Scalar> Force;

  template <template <typename Scalar> class Model>
  ContactData6DTpl(Model<Scalar>* const model,
                   pinocchio::DataTpl<Scalar>* const data)
      : Base(model, data),
        rMf(SE3::Identity()),
        lwaMl(SE3::Identity()),
        v(Motion::Zero()),
        a0_local(Motion::Zero()),
        f_local(Force::Zero()),
        da0_local_dx(6, model->get_state()->get_ndx()),
        fJf(6, model->get_state()->get_nv()),
        v_partial_dq(6, model->get_state()->get_nv()),
        a_partial_dq(6, model->get_state()->get_nv()),
        a_partial_dv(6, model->get_state()->get_nv()),
        a_partial_da(6, model->get_state()->get_nv()),
        fJf_df(6, model->get_state()->get_nv()) {
    frame = model->get_id();
    jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
    fXj = jMf.inverse().toActionMatrix();
    da0_local_dx.setZero();
    fJf.setZero();
    v_partial_dq.setZero();
    a_partial_dq.setZero();
    a_partial_dv.setZero();
    a_partial_da.setZero();
    av_world_skew.setZero();
    aw_world_skew.setZero();
    av_skew.setZero();
    aw_skew.setZero();
    fv_skew.setZero();
    fw_skew.setZero();
    rMf_Jlog6.setZero();
    fJf_df.setZero();
  }

  using Base::a0;
  using Base::da0_dx;
  using Base::df_du;
  using Base::df_dx;
  using Base::f;
  using Base::frame;
  using Base::fXj;
  using Base::Jc;
  using Base::jMf;
  using Base::pinocchio;

  SE3 rMf;
  SE3 lwaMl;
  Motion v;
  Motion a0_local;
  Force f_local;
  Matrix6xs da0_local_dx;
  MatrixXs fJf;
  Matrix6xs v_partial_dq;
  Matrix6xs a_partial_dq;
  Matrix6xs a_partial_dv;
  Matrix6xs a_partial_da;
  Matrix3s av_world_skew;
  Matrix3s aw_world_skew;
  Matrix3s av_skew;
  Matrix3s aw_skew;
  Matrix3s fv_skew;
  Matrix3s fw_skew;
  Matrix6s rMf_Jlog6;
  MatrixXs fJf_df;
};

}  // namespace crocoddyl
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/multibody/contacts/contact-6d.hxx"

#endif  // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_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_CONTACT_6D_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
12
13			#include <pinocchio/multibody/data.hpp>
14			#include <pinocchio/spatial/motion.hpp>
15
16			#include "crocoddyl/core/utils/deprecate.hpp"
17			#include "crocoddyl/multibody/contact-base.hpp"
18			#include "crocoddyl/multibody/fwd.hpp"
19
20			namespace crocoddyl {
21
22			template <typename _Scalar>
23			class ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
24			public:
25			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26
27			typedef _Scalar Scalar;
28			typedef MathBaseTpl<Scalar> MathBase;
29			typedef ContactModelAbstractTpl<Scalar> Base;
30			typedef ContactData6DTpl<Scalar> Data;
31			typedef StateMultibodyTpl<Scalar> StateMultibody;
32			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
33			typedef pinocchio::SE3Tpl<Scalar> SE3;
34			typedef typename MathBase::Vector2s Vector2s;
35			typedef typename MathBase::Vector3s Vector3s;
36			typedef typename MathBase::VectorXs VectorXs;
37			typedef typename MathBase::Matrix3s Matrix3s;
38
39			/**
40			* @brief Initialize the 6d contact model
41			*
42			* To learn more about the computation of the contact derivatives in different
43			* frames see
44			* S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
45			* Frames: Application to Polishing with Talos, ICHR 2022
46			*
47			* @param[in] state State of the multibody system
48			* @param[in] id Reference frame id of the contact
49			* @param[in] pref Contact placement used for the Baumgarte stabilization
50			* @param[in] type Type of contact
51			* @param[in] nu Dimension of the control vector
52			* @param[in] gains Baumgarte stabilization gains
53			*/
54			ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
55			const pinocchio::FrameIndex id, const SE3& pref,
56			const pinocchio::ReferenceFrame type, const std::size_t nu,
57			const Vector2s& gains = Vector2s::Zero());
58
59			/**
60			* @brief Initialize the 6d contact model
61			*
62			* The default `nu` is obtained from `StateAbstractTpl::get_nv()`. To learn
63			* more about the computation of the contact derivatives in different frames
64			* see
65			* S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
66			* Frames: Application to Polishing with Talos, ICHR 2022
67			*
68			* @param[in] state State of the multibody system
69			* @param[in] id Reference frame id of the contact
70			* @param[in] pref Contact placement used for the Baumgarte stabilization
71			* @param[in] type Type of contact
72			* @param[in] gains Baumgarte stabilization gains
73			*/
74			ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
75			const pinocchio::FrameIndex id, const SE3& pref,
76			const pinocchio::ReferenceFrame type,
77			const Vector2s& gains = Vector2s::Zero());
78
79			DEPRECATED(
80			"Use constructor that passes the type type of contact, this assumes is "
81			"pinocchio::LOCAL",
82			ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
83			const pinocchio::FrameIndex id, const SE3& pref,
84			const std::size_t nu,
85			const Vector2s& gains = Vector2s::Zero());)
86			DEPRECATED(
87			"Use constructor that passes the type type of contact, this assumes is "
88			"pinocchio::LOCAL",
89			ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
90			const pinocchio::FrameIndex id, const SE3& pref,
91			const Vector2s& gains = Vector2s::Zero());)
92			virtual ~ContactModel6DTpl();
93
94			/**
95			* @brief Compute the 3d contact Jacobian and drift
96			*
97			* @param[in] data 3d contact data
98			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
99			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
100			*/
101			virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data,
102			const Eigen::Ref<const VectorXs>& x);
103
104			/**
105			* @brief Compute the derivatives of the 6d contact holonomic constraint
106			*
107			* @param[in] data 6d contact data
108			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
109			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
110			*/
111			virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
112			const Eigen::Ref<const VectorXs>& x);
113
114			/**
115			* @brief Convert the force into a stack of spatial forces
116			*
117			* @param[in] data 6d contact data
118			* @param[in] force 6d force
119			*/
120			virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data,
121			const VectorXs& force);
122
123			/**
124			* @brief Create the 6d contact data
125			*/
126			virtual boost::shared_ptr<ContactDataAbstract> createData(
127			pinocchio::DataTpl<Scalar>* const data);
128
129			/**
130			* @brief Return the reference frame placement
131			*/
132			const SE3& get_reference() const;
133
134			/**
135			* @brief Return the Baumgarte stabilization gains
136			*/
137			const Vector2s& get_gains() const;
138
139			/**
140			* @brief Modify the reference frame placement
141			*/
142			void set_reference(const SE3& reference);
143
144			/**
145			* @brief Print relevant information of the 6d contact model
146			*
147			* @param[out] os Output stream object
148			*/
149			virtual void print(std::ostream& os) const;
150
151			protected:
152			using Base::id_;
153			using Base::nc_;
154			using Base::nu_;
155			using Base::state_;
156			using Base::type_;
157
158			private:
159			SE3 pref_; //!< Contact placement used for the Baumgarte stabilization
160			Vector2s gains_; //!< Baumgarte stabilization gains
161			};
162
163			template <typename _Scalar>
164			struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
165			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166
167			typedef _Scalar Scalar;
168			typedef MathBaseTpl<Scalar> MathBase;
169			typedef ContactDataAbstractTpl<Scalar> Base;
170			typedef typename MathBase::Matrix3s Matrix3s;
171			typedef typename MathBase::Matrix6xs Matrix6xs;
172			typedef typename MathBase::Matrix6s Matrix6s;
173			typedef typename MathBase::MatrixXs MatrixXs;
174			typedef typename pinocchio::SE3Tpl<Scalar> SE3;
175			typedef typename pinocchio::MotionTpl<Scalar> Motion;
176			typedef typename pinocchio::ForceTpl<Scalar> Force;
177
178			template <template <typename Scalar> class Model>
179		63021	ContactData6DTpl(Model<Scalar>* const model,
180			pinocchio::DataTpl<Scalar>* const data)
181			: Base(model, data),
182			rMf(SE3::Identity()),
183			lwaMl(SE3::Identity()),
184			v(Motion::Zero()),
185			a0_local(Motion::Zero()),
186			f_local(Force::Zero()),
187		63021	da0_local_dx(6, model->get_state()->get_ndx()),
188		63021	fJf(6, model->get_state()->get_nv()),
189		63021	v_partial_dq(6, model->get_state()->get_nv()),
190		63021	a_partial_dq(6, model->get_state()->get_nv()),
191		63021	a_partial_dv(6, model->get_state()->get_nv()),
192		63021	a_partial_da(6, model->get_state()->get_nv()),
193	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	441147	fJf_df(6, model->get_state()->get_nv()) {
194		63021	frame = model->get_id();
195	✓✗	63021	jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
196	✓✗✓✗	63021	fXj = jMf.inverse().toActionMatrix();
197	✓✗	63021	da0_local_dx.setZero();
198	✓✗	63021	fJf.setZero();
199	✓✗	63021	v_partial_dq.setZero();
200	✓✗	63021	a_partial_dq.setZero();
201	✓✗	63021	a_partial_dv.setZero();
202	✓✗	63021	a_partial_da.setZero();
203	✓✗	63021	av_world_skew.setZero();
204	✓✗	63021	aw_world_skew.setZero();
205	✓✗	63021	av_skew.setZero();
206	✓✗	63021	aw_skew.setZero();
207	✓✗	63021	fv_skew.setZero();
208	✓✗	63021	fw_skew.setZero();
209	✓✗	63021	rMf_Jlog6.setZero();
210	✓✗	63021	fJf_df.setZero();
211		63021	}
212
213			using Base::a0;
214			using Base::da0_dx;
215			using Base::df_du;
216			using Base::df_dx;
217			using Base::f;
218			using Base::frame;
219			using Base::fXj;
220			using Base::Jc;
221			using Base::jMf;
222			using Base::pinocchio;
223
224			SE3 rMf;
225			SE3 lwaMl;
226			Motion v;
227			Motion a0_local;
228			Force f_local;
229			Matrix6xs da0_local_dx;
230			MatrixXs fJf;
231			Matrix6xs v_partial_dq;
232			Matrix6xs a_partial_dq;
233			Matrix6xs a_partial_dv;
234			Matrix6xs a_partial_da;
235			Matrix3s av_world_skew;
236			Matrix3s aw_world_skew;
237			Matrix3s av_skew;
238			Matrix3s aw_skew;
239			Matrix3s fv_skew;
240			Matrix3s fw_skew;
241			Matrix6s rMf_Jlog6;
242			MatrixXs fJf_df;
243			};
244
245			} // namespace crocoddyl
246			/* --- Details -------------------------------------------------------------- */
247			/* --- Details -------------------------------------------------------------- */
248			/* --- Details -------------------------------------------------------------- */
249			#include "crocoddyl/multibody/contacts/contact-6d.hxx"
250
251			#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_