GCC Code Coverage Report

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_IMPULSE_BASE_HPP_
11			#define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
12
13			#include "crocoddyl/core/utils/deprecate.hpp"
14			#include "crocoddyl/multibody/force-base.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16			#include "crocoddyl/multibody/states/multibody.hpp"
17
18			namespace crocoddyl {
19
20			template <typename _Scalar>
21			class ImpulseModelAbstractTpl {
22			public:
23			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24
25			typedef _Scalar Scalar;
26			typedef MathBaseTpl<Scalar> MathBase;
27			typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
28			typedef StateMultibodyTpl<Scalar> StateMultibody;
29			typedef typename MathBase::VectorXs VectorXs;
30			typedef typename MathBase::MatrixXs MatrixXs;
31
32			ImpulseModelAbstractTpl(boost::shared_ptr<StateMultibody> state,
33			const pinocchio::ReferenceFrame type,
34			const std::size_t nc);
35
36			DEPRECATED(
37			"Use constructor that passes the type type of contact, this assumes is "
38			"pinocchio::LOCAL",
39			ImpulseModelAbstractTpl(boost::shared_ptr<StateMultibody> state,
40			const std::size_t nc);)
41			virtual ~ImpulseModelAbstractTpl();
42
43			virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data,
44			const Eigen::Ref<const VectorXs>& x) = 0;
45			virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data,
46			const Eigen::Ref<const VectorXs>& x) = 0;
47
48			virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data,
49			const VectorXs& force) = 0;
50			void updateForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data,
51			const MatrixXs& df_dx) const;
52			void setZeroForce(const boost::shared_ptr<ImpulseDataAbstract>& data) const;
53			void setZeroForceDiff(
54			const boost::shared_ptr<ImpulseDataAbstract>& data) const;
55
56			virtual boost::shared_ptr<ImpulseDataAbstract> createData(
57			pinocchio::DataTpl<Scalar>* const data);
58
59			const boost::shared_ptr<StateMultibody>& get_state() const;
60			std::size_t get_nc() const;
61			DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
62			std::size_t get_nu() const;
63
64			/**
65			* @brief Return the reference frame id
66			*/
67			pinocchio::FrameIndex get_id() const;
68
69			/**
70			* @brief Modify the reference frame id
71			*/
72			void set_id(const pinocchio::FrameIndex id);
73
74			/**
75			* @brief Modify the type of contact
76			*/
77			void set_type(const pinocchio::ReferenceFrame type);
78
79			/**
80			* @brief Return the type of contact
81			*/
82			pinocchio::ReferenceFrame get_type() const;
83
84			/**
85			* @brief Print information on the impulse model
86			*/
87			template <class Scalar>
88			friend std::ostream& operator<<(std::ostream& os,
89			const ImpulseModelAbstractTpl<Scalar>& model);
90
91			/**
92			* @brief Print relevant information of the impulse model
93			*
94			* @param[out] os Output stream object
95			*/
96			virtual void print(std::ostream& os) const;
97
98			protected:
99			boost::shared_ptr<StateMultibody> state_;
100			std::size_t nc_;
101			pinocchio::FrameIndex id_; //!< Reference frame id of the contact
102			pinocchio::ReferenceFrame type_; //!< Type of contact
103			};
104
105			template <typename _Scalar>
106			struct ImpulseDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
107			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108
109			typedef _Scalar Scalar;
110			typedef MathBaseTpl<Scalar> MathBase;
111			typedef ForceDataAbstractTpl<Scalar> Base;
112			typedef typename MathBase::VectorXs VectorXs;
113			typedef typename MathBase::MatrixXs MatrixXs;
114			typedef typename pinocchio::SE3Tpl<Scalar> SE3;
115
116			template <template <typename Scalar> class Model>
117		20896	ImpulseDataAbstractTpl(Model<Scalar>* const model,
118			pinocchio::DataTpl<Scalar>* const data)
119			: Base(model, data),
120	1/2 ✓ Branch 5 taken 10460 times. ✗ Branch 6 not taken.	20896	dv0_dq(model->get_nc(), model->get_state()->get_nv()),
121	2/4 ✓ Branch 2 taken 10460 times. ✗ Branch 3 not taken. ✓ Branch 11 taken 10460 times. ✗ Branch 12 not taken.	41792	dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
122	1/2 ✓ Branch 1 taken 10460 times. ✗ Branch 2 not taken.	20896	dv0_dq.setZero();
123	1/2 ✓ Branch 1 taken 10460 times. ✗ Branch 2 not taken.	20896	dtau_dq.setZero();
124		20896	}
125		10544	virtual ~ImpulseDataAbstractTpl() {}
126
127			using Base::df_dx;
128			using Base::f;
129			using Base::frame;
130			using Base::Jc;
131			using Base::jMf;
132			using Base::pinocchio;
133
134			typename SE3::ActionMatrixType fXj;
135			MatrixXs dv0_dq;
136			MatrixXs dtau_dq;
137			};
138
139			} // namespace crocoddyl
140
141			/* --- Details -------------------------------------------------------------- */
142			/* --- Details -------------------------------------------------------------- */
143			/* --- Details -------------------------------------------------------------- */
144			#include "crocoddyl/multibody/impulse-base.hxx"
145
146			#endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
147