GCC Code Coverage Report

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