GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/control-gravity.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 40 42 95.2%
Branches: 23 56 41.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2022, 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 boost::shared_ptr<StateMultibody> state, const std::size_t nu)
17 99 : Base(state, state->get_nv(), nu, true, false),
18
2/4
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
✓ Branch 11 taken 99 times.
✗ Branch 12 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 boost::shared_ptr<StateMultibody> state)
29 1 : Base(state, state->get_nv(), state->get_nv(), true, false),
30
2/4
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 2 pin_model_(*state->get_pinocchio()) {}
31
32 template <typename Scalar>
33 204 ResidualModelControlGravTpl<Scalar>::~ResidualModelControlGravTpl() {}
34
35 template <typename Scalar>
36 1498 void ResidualModelControlGravTpl<Scalar>::calc(
37 const boost::shared_ptr<ResidualDataAbstract> &data,
38 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
39 1498 Data *d = static_cast<Data *>(data.get());
40
41 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
42
1/2
✓ Branch 3 taken 1498 times.
✗ Branch 4 not taken.
 1498 x.head(state_->get_nq());
43
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 -
44
1/2
✓ Branch 1 taken 1498 times.
✗ Branch 2 not taken.
 1498 pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q);
45 1498 }
46
47 template <typename Scalar>
48 968 void ResidualModelControlGravTpl<Scalar>::calc(
49 const boost::shared_ptr<ResidualDataAbstract> &data,
50 const Eigen::Ref<const VectorXs> &x) {
51 968 Data *d = static_cast<Data *>(data.get());
52
53 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
54
1/2
✓ Branch 3 taken 968 times.
✗ Branch 4 not taken.
 968 x.head(state_->get_nq());
55
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);
56 968 }
57
58 template <typename Scalar>
59 45 void ResidualModelControlGravTpl<Scalar>::calcDiff(
60 const boost::shared_ptr<ResidualDataAbstract> &data,
61 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
62 45 Data *d = static_cast<Data *>(data.get());
63
64 // Compute the derivatives of the residual residual
65 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
66
1/2
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
 45 x.head(state_->get_nq());
67 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
68
1/2
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 data->Rx.leftCols(state_->get_nv());
69
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q,
70 Rq);
71
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 Rq *= -1;
72
1/2
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
 45 data->Ru = d->actuation->dtau_du;
73 45 }
74
75 template <typename Scalar>
76 27 void ResidualModelControlGravTpl<Scalar>::calcDiff(
77 const boost::shared_ptr<ResidualDataAbstract> &data,
78 const Eigen::Ref<const VectorXs> &x) {
79 27 Data *d = static_cast<Data *>(data.get());
80
81 // Compute the derivatives of the residual residual
82 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
83
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 x.head(state_->get_nq());
84 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
85
1/2
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 data->Rx.leftCols(state_->get_nv());
86
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q,
87 Rq);
88
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 Rq *= -1;
89 27 }
90
91 template <typename Scalar>
92 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
93 2845 ResidualModelControlGravTpl<Scalar>::createData(
94 DataCollectorAbstract *const data) {
95 ✗ return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
96
1/2
✓ Branch 2 taken 2845 times.
✗ Branch 3 not taken.
 2845 data);
97 }
98
99 template <typename Scalar>
100 27 void ResidualModelControlGravTpl<Scalar>::print(std::ostream &os) const {
101 27 os << "ResidualModelControlGrav";
102 27 }
103
104 } // namespace crocoddyl
105