GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/control-gravity.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 39 46 84.8%
Branches: 31 74 41.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <pinocchio/algorithm/rnea-derivatives.hpp>
10 #include <pinocchio/algorithm/rnea.hpp>
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 99 ResidualModelControlGravTpl<Scalar>::ResidualModelControlGravTpl(
16 std::shared_ptr<StateMultibody> state, const std::size_t nu)
17 99 : Base(state, state->get_nv(), nu, true, false),
18
3/6
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 99 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 99 times.
✗ Branch 14 not taken.
 99 pin_model_(*state->get_pinocchio()) {
19
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 99 times.
 99 if (nu_ == 0) {
20 ✗ throw_pretty("Invalid argument: "
21 << "it seems to be an autonomous system, if so, don't add "
22 "this residual function");
23 }
24 99 }
25
26 template <typename Scalar>
27 1 ResidualModelControlGravTpl<Scalar>::ResidualModelControlGravTpl(
28 std::shared_ptr<StateMultibody> state)
29 2 : Base(state, state->get_nv(), state->get_nv(), true, false),
30
3/6
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 pin_model_(*state->get_pinocchio()) {}
31
32 template <typename Scalar>
33 1498 void ResidualModelControlGravTpl<Scalar>::calc(
34 const std::shared_ptr<ResidualDataAbstract> &data,
35 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
36 1498 Data *d = static_cast<Data *>(data.get());
37
38 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
39
2/4
✓ Branch 2 taken 1498 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1498 times.
✗ Branch 6 not taken.
 1498 x.head(state_->get_nq());
40
2/4
✓ Branch 1 taken 1498 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1498 times.
✗ Branch 6 not taken.
 1498 data->r = d->actuation->tau -
41
1/2
✓ Branch 1 taken 1498 times.
✗ Branch 2 not taken.
 1498 pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q);
42 1498 }
43
44 template <typename Scalar>
45 968 void ResidualModelControlGravTpl<Scalar>::calc(
46 const std::shared_ptr<ResidualDataAbstract> &data,
47 const Eigen::Ref<const VectorXs> &x) {
48 968 Data *d = static_cast<Data *>(data.get());
49
50 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
51
2/4
✓ Branch 2 taken 968 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 968 times.
✗ Branch 6 not taken.
 968 x.head(state_->get_nq());
52
3/6
✓ Branch 1 taken 968 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 968 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 968 times.
✗ Branch 9 not taken.
 968 data->r = -pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q);
53 968 }
54
55 template <typename Scalar>
56 45 void ResidualModelControlGravTpl<Scalar>::calcDiff(
57 const std::shared_ptr<ResidualDataAbstract> &data,
58 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
59 45 Data *d = static_cast<Data *>(data.get());
60
61 // Compute the derivatives of the residual residual
62 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
63
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 45 x.head(state_->get_nq());
64 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
65
2/4
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 45 times.
✗ Branch 7 not taken.
 45 data->Rx.leftCols(state_->get_nv());
66
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q,
67 Rq);
68
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 Rq *= Scalar(-1);
69
1/2
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
 45 data->Ru = d->actuation->dtau_du;
70 45 }
71
72 template <typename Scalar>
73 27 void ResidualModelControlGravTpl<Scalar>::calcDiff(
74 const std::shared_ptr<ResidualDataAbstract> &data,
75 const Eigen::Ref<const VectorXs> &x) {
76 27 Data *d = static_cast<Data *>(data.get());
77
78 // Compute the derivatives of the residual residual
79 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
80
2/4
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
 27 x.head(state_->get_nq());
81 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
82
2/4
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
 27 data->Rx.leftCols(state_->get_nv());
83
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q,
84 Rq);
85
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 Rq *= Scalar(-1);
86 27 }
87
88 template <typename Scalar>
89 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
90 2845 ResidualModelControlGravTpl<Scalar>::createData(
91 DataCollectorAbstract *const data) {
92 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
93
1/2
✓ Branch 2 taken 2845 times.
✗ Branch 3 not taken.
 2845 data);
94 }
95
96 template <typename Scalar>
97 template <typename NewScalar>
98 ResidualModelControlGravTpl<NewScalar>
99 ✗ ResidualModelControlGravTpl<Scalar>::cast() const {
100 typedef ResidualModelControlGravTpl<NewScalar> ReturnType;
101 typedef StateMultibodyTpl<NewScalar> StateType;
102 ✗ ReturnType ret(
103 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
104 ✗ nu_);
105 ✗ return ret;
106 }
107
108 template <typename Scalar>
109 27 void ResidualModelControlGravTpl<Scalar>::print(std::ostream &os) const {
110 27 os << "ResidualModelControlGrav";
111 27 }
112
113 } // namespace crocoddyl
114