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