GCC Code Coverage Report


Directory: ./
File: include/crocoddyl/multibody/residuals/impulse-com.hpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 14 15 93.3%
Branches: 9 26 34.6%

Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
11
12 #include "crocoddyl/core/residual-base.hpp"
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/multibody/data/impulses.hpp"
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/impulse-base.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18
19 namespace crocoddyl {
20
21 /**
22 * @brief Impulse CoM residual
23 *
24 * This residual function is defined as
25 * \f$\mathbf{r}=\mathbf{J}_{com}*(\mathbf{v}_{next}-\mathbf{v})\f$,
26 * \f$\mathbf{J}_{com}\in\mathbb{R}^{3\times nv}\f$ is the CoM Jacobian, and
27 * \f$\mathbf{v}_{next},\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\f$ are the
28 * generalized velocities after and before the impulse, respectively. Note that
29 * the dimension of the residual vector is 3. Furthermore, the Jacobians of the
30 * residual function are computed analytically.
31 *
32 * As described in `ResidualModelAbstractTpl()`, the residual value and its
33 * Jacobians are calculated by `calc` and `calcDiff`, respectively.
34 *
35 * \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
36 */
37 template <typename _Scalar>
38 class ResidualModelImpulseCoMTpl : public ResidualModelAbstractTpl<_Scalar> {
39 public:
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41
42 typedef _Scalar Scalar;
43 typedef MathBaseTpl<Scalar> MathBase;
44 typedef ResidualModelAbstractTpl<Scalar> Base;
45 typedef ResidualDataImpulseCoMTpl<Scalar> Data;
46 typedef StateMultibodyTpl<Scalar> StateMultibody;
47 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
48 typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
49 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
50 typedef typename MathBase::VectorXs VectorXs;
51
52 /**
53 * @brief Initialize the impulse CoM residual model
54 *
55 * @param[in] state State of the multibody system
56 */
57 ResidualModelImpulseCoMTpl(std::shared_ptr<StateMultibody> state);
58 virtual ~ResidualModelImpulseCoMTpl();
59
60 /**
61 * @brief Compute the impulse CoM residual
62 *
63 * @param[in] data Impulse CoM residual data
64 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
65 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
66 */
67 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
68 const Eigen::Ref<const VectorXs>& x,
69 const Eigen::Ref<const VectorXs>& u);
70
71 /**
72 * @brief Compute the Jacobians of the impulse CoM residual
73 *
74 * @param[in] data Impulse CoM residual data
75 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
76 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
77 */
78 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
79 const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u);
81
82 /**
83 * @brief Create the impulse CoM residual data
84 */
85 virtual std::shared_ptr<ResidualDataAbstract> createData(
86 DataCollectorAbstract* const data);
87
88 /**
89 * @brief Print relevant information of the impulse-com residual
90 *
91 * @param[out] os Output stream object
92 */
93 virtual void print(std::ostream& os) const;
94
95 protected:
96 using Base::nu_;
97 using Base::state_;
98 using Base::u_dependent_;
99
100 private:
101 std::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
102 };
103
104 template <typename _Scalar>
105 struct ResidualDataImpulseCoMTpl : public ResidualDataAbstractTpl<_Scalar> {
106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
107
108 typedef _Scalar Scalar;
109 typedef MathBaseTpl<Scalar> MathBase;
110 typedef ResidualDataAbstractTpl<Scalar> Base;
111 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
112 typedef StateMultibodyTpl<Scalar> StateMultibody;
113 typedef typename MathBase::MatrixXs MatrixXs;
114 typedef typename MathBase::Matrix3xs Matrix3xs;
115
116 template <template <typename Scalar> class Model>
117 1080 ResidualDataImpulseCoMTpl(Model<Scalar>* const model,
118 DataCollectorAbstract* const data)
119 : Base(model, data),
120
1/2
✓ Branch 4 taken 1080 times.
✗ Branch 5 not taken.
1080 dvc_dq(3, model->get_state()->get_nv()),
121
2/4
✓ Branch 9 taken 1080 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1080 times.
✗ Branch 13 not taken.
2160 ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
122
1/2
✓ Branch 1 taken 1080 times.
✗ Branch 2 not taken.
1080 dvc_dq.setZero();
123
1/2
✓ Branch 1 taken 1080 times.
✗ Branch 2 not taken.
1080 ddv_dv.setZero();
124 1080 const std::shared_ptr<StateMultibody>& state =
125 std::static_pointer_cast<StateMultibody>(model->get_state());
126
1/2
✓ Branch 1 taken 1080 times.
✗ Branch 2 not taken.
1080 pinocchio_internal =
127
1/2
✓ Branch 4 taken 1080 times.
✗ Branch 5 not taken.
2160 pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
128 // Check that proper shared data has been passed
129 1080 DataCollectorMultibodyInImpulseTpl<Scalar>* d =
130
1/2
✓ Branch 0 taken 1080 times.
✗ Branch 1 not taken.
1080 dynamic_cast<DataCollectorMultibodyInImpulseTpl<Scalar>*>(shared);
131
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1080 times.
1080 if (d == NULL) {
132 throw_pretty(
133 "Invalid argument: the shared data should be derived from "
134 "DataCollectorMultibodyInImpulse");
135 }
136 1080 pinocchio = d->pinocchio;
137 1080 impulses = d->impulses;
138 1080 }
139
140 pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
141 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
142 impulses; //!< Impulses data
143 Matrix3xs dvc_dq; //!< Jacobian of the CoM velocity
144 MatrixXs ddv_dv; //!< Jacobian of the CoM velocity
145 pinocchio::DataTpl<Scalar>
146 pinocchio_internal; //!< Pinocchio data for internal computation
147 using Base::r;
148 using Base::Ru;
149 using Base::Rx;
150 using Base::shared;
151 };
152
153 } // namespace crocoddyl
154
155 /* --- Details -------------------------------------------------------------- */
156 /* --- Details -------------------------------------------------------------- */
157 /* --- Details -------------------------------------------------------------- */
158 #include "crocoddyl/multibody/residuals/impulse-com.hxx"
159
160 #endif // CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
161