GCC Code Coverage Report

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