GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/impulses/impulse-6d.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 26 27 96.3%
Branches: 41 82 50.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_IMPULSES_IMPULSE_6D_HPP_
11 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
12
13 #include <pinocchio/multibody/data.hpp>
14 #include <pinocchio/spatial/motion.hpp>
15
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/multibody/impulse-base.hpp"
18
19 namespace crocoddyl {
20
21 template <typename _Scalar>
22 class ImpulseModel6DTpl : public ImpulseModelAbstractTpl<_Scalar> {
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 ✗ CROCODDYL_DERIVED_CAST(ImpulseModelBase, ImpulseModel6DTpl)
26
27 typedef _Scalar Scalar;
28 typedef MathBaseTpl<Scalar> MathBase;
29 typedef ImpulseModelAbstractTpl<Scalar> Base;
30 typedef ImpulseData6DTpl<Scalar> Data;
31 typedef StateMultibodyTpl<Scalar> StateMultibody;
32 typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
33 typedef typename MathBase::Vector2s Vector2s;
34 typedef typename MathBase::Vector3s Vector3s;
35 typedef typename MathBase::VectorXs VectorXs;
36 typedef typename MathBase::MatrixXs MatrixXs;
37 typedef typename MathBase::Matrix3s Matrix3s;
38
39 /**
40 * @brief Initialize the 6d impulse model
41 *
42 * @param[in] state State of the multibody system
43 * @param[in] id Reference frame id of the impulse
44 * @param[in] type Type of impulse (default LOCAL)
45 */
46 ImpulseModel6DTpl(
47 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
48 const pinocchio::ReferenceFrame type = pinocchio::ReferenceFrame::LOCAL);
49 416 virtual ~ImpulseModel6DTpl() = default;
50
51 /**
52 * @brief Compute the 3d impulse Jacobian
53 *
54 * @param[in] data 3d impulse data
55 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
56 */
57 virtual void calc(const std::shared_ptr<ImpulseDataAbstract>& data,
58 const Eigen::Ref<const VectorXs>& x) override;
59
60 /**
61 * @brief Compute the derivatives of the 3d impulse holonomic constraint
62 *
63 * @param[in] data 3d impulse data
64 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
65 */
66 virtual void calcDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
67 const Eigen::Ref<const VectorXs>& x) override;
68
69 /**
70 * @brief Convert the force into a stack of spatial forces
71 *
72 * @param[in] data 3d impulse data
73 * @param[in] force 3d impulse
74 */
75 virtual void updateForce(const std::shared_ptr<ImpulseDataAbstract>& data,
76 const VectorXs& force) override;
77
78 /**
79 * @brief Create the 3d impulse data
80 */
81 virtual std::shared_ptr<ImpulseDataAbstract> createData(
82 pinocchio::DataTpl<Scalar>* const data) override;
83
84 /**
85 * @brief Cast the impulse-6d model to a different scalar type.
86 *
87 * It is useful for operations requiring different precision or scalar types.
88 *
89 * @tparam NewScalar The new scalar type to cast to.
90 * @return ImpulseModel6DTpl<NewScalar> An impulse model with the
91 * new scalar type.
92 */
93 template <typename NewScalar>
94 ImpulseModel6DTpl<NewScalar> cast() const;
95
96 /**
97 * @brief Print relevant information of the 6d impulse model
98 *
99 * @param[out] os Output stream object
100 */
101 virtual void print(std::ostream& os) const override;
102
103 protected:
104 using Base::id_;
105 using Base::state_;
106 using Base::type_;
107 };
108
109 template <typename _Scalar>
110 struct ImpulseData6DTpl : public ImpulseDataAbstractTpl<_Scalar> {
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112 typedef _Scalar Scalar;
113 typedef MathBaseTpl<Scalar> MathBase;
114 typedef ImpulseDataAbstractTpl<Scalar> Base;
115 typedef typename MathBase::Matrix3s Matrix3s;
116 typedef typename MathBase::Matrix6xs Matrix6xs;
117 typedef typename MathBase::MatrixXs MatrixXs;
118 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
119 typedef typename pinocchio::MotionTpl<Scalar> Motion;
120 typedef typename pinocchio::ForceTpl<Scalar> Force;
121
122 template <template <typename Scalar> class Model>
123 6965 ImpulseData6DTpl(Model<Scalar>* const model,
124 pinocchio::DataTpl<Scalar>* const data)
125 : Base(model, data),
126 6965 lwaMl(SE3::Identity()),
127
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 v0(Motion::Zero()),
128
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 f_local(Force::Zero()),
129
3/6
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6939 times.
✗ Branch 9 not taken.
 6965 dv0_local_dq(6, model->get_state()->get_nv()),
130
3/6
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6939 times.
✗ Branch 9 not taken.
 6965 fJf(6, model->get_state()->get_nv()),
131
3/6
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6939 times.
✗ Branch 9 not taken.
 6965 v_partial_dq(6, model->get_state()->get_nv()),
132
3/6
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6939 times.
✗ Branch 9 not taken.
 6965 v_partial_dv(6, model->get_state()->get_nv()),
133
10/20
✓ Branch 2 taken 6939 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6939 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6939 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6939 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6939 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6939 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 6939 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 6939 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6939 times.
✗ Branch 31 not taken.
 13930 fJf_df(6, model->get_state()->get_nv()) {
134
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 frame = model->get_id();
135 6965 jMf =
136
4/8
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6939 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6939 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6939 times.
✗ Branch 14 not taken.
 6965 model->get_state()->get_pinocchio()->frames[model->get_id()].placement;
137
2/4
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6939 times.
✗ Branch 5 not taken.
 6965 fXj = jMf.inverse().toActionMatrix();
138
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 fJf.setZero();
139
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 v_partial_dq.setZero();
140
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 v_partial_dv.setZero();
141
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 vv_skew.setZero();
142
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 vw_skew.setZero();
143
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 vv_world_skew.setZero();
144
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 vw_world_skew.setZero();
145
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 fv_skew.setZero();
146
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 fw_skew.setZero();
147
1/2
✓ Branch 1 taken 6939 times.
✗ Branch 2 not taken.
 6965 fJf_df.setZero();
148 6965 }
149 13854 virtual ~ImpulseData6DTpl() = default;
150
151 using Base::df_dx;
152 using Base::dv0_dq;
153 using Base::f;
154 using Base::frame;
155 using Base::fXj;
156 using Base::Jc;
157 using Base::jMf;
158 using Base::pinocchio;
159
160 SE3 lwaMl;
161 Motion v0;
162 Force f_local;
163 Matrix6xs dv0_local_dq;
164 Matrix6xs fJf;
165 Matrix6xs v_partial_dq;
166 Matrix6xs v_partial_dv;
167 Matrix3s vv_skew;
168 Matrix3s vw_skew;
169 Matrix3s vv_world_skew;
170 Matrix3s vw_world_skew;
171 Matrix3s fv_skew;
172 Matrix3s fw_skew;
173 MatrixXs fJf_df;
174 };
175
176 } // namespace crocoddyl
177
178 /* --- Details -------------------------------------------------------------- */
179 /* --- Details -------------------------------------------------------------- */
180 /* --- Details -------------------------------------------------------------- */
181 #include "crocoddyl/multibody/impulses/impulse-6d.hxx"
182
183 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModel6DTpl)
184 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseData6DTpl)
185
186 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
187