GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-control-gravity.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 39 43 90.7%
Branches: 23 56 41.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, 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 #include "crocoddyl/multibody/residuals/contact-control-gravity.hpp"
13
14 namespace crocoddyl {
15
16 template <typename Scalar>
17 39 ResidualModelContactControlGravTpl<Scalar>::ResidualModelContactControlGravTpl(
18 boost::shared_ptr<StateMultibody> state, const std::size_t nu)
19 39 : Base(state, state->get_nv(), nu, true, false, true),
20
2/4
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 11 taken 39 times.
✗ Branch 12 not taken.
 39 pin_model_(*state->get_pinocchio()) {
21
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 39 times.
 39 if (nu_ == 0) {
22 ✗ throw_pretty("Invalid argument: "
23 << "it seems to be an autonomous system, if so, don't add "
24 "this residual function");
25 }
26 39 }
27
28 template <typename Scalar>
29 1 ResidualModelContactControlGravTpl<Scalar>::ResidualModelContactControlGravTpl(
30 boost::shared_ptr<StateMultibody> state)
31 1 : Base(state, state->get_nv(), state->get_nv(), true, false, true),
32
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()) {}
33
34 template <typename Scalar>
35 84 ResidualModelContactControlGravTpl<
36 84 Scalar>::~ResidualModelContactControlGravTpl() {}
37
38 template <typename Scalar>
39 2772 void ResidualModelContactControlGravTpl<Scalar>::calc(
40 const boost::shared_ptr<ResidualDataAbstract> &data,
41 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
42 2772 Data *d = static_cast<Data *>(data.get());
43
44 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
45
1/2
✓ Branch 3 taken 2772 times.
✗ Branch 4 not taken.
 2772 x.head(state_->get_nq());
46
2/4
✓ Branch 2 taken 2772 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2772 times.
✗ Branch 7 not taken.
 2772 data->r = d->actuation->tau - pinocchio::computeStaticTorque(
47
1/2
✓ Branch 1 taken 2772 times.
✗ Branch 2 not taken.
 2772 pin_model_, d->pinocchio, q, d->fext);
48 2772 }
49
50 template <typename Scalar>
51 2030 void ResidualModelContactControlGravTpl<Scalar>::calc(
52 const boost::shared_ptr<ResidualDataAbstract> &data,
53 const Eigen::Ref<const VectorXs> &x) {
54 2030 Data *d = static_cast<Data *>(data.get());
55
56 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
57
1/2
✓ Branch 3 taken 2030 times.
✗ Branch 4 not taken.
 2030 x.head(state_->get_nq());
58
3/6
✓ Branch 1 taken 2030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2030 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2030 times.
✗ Branch 9 not taken.
 2030 data->r = -pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q);
59 2030 }
60
61 template <typename Scalar>
62 39 void ResidualModelContactControlGravTpl<Scalar>::calcDiff(
63 const boost::shared_ptr<ResidualDataAbstract> &data,
64 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
65 39 Data *d = static_cast<Data *>(data.get());
66
67 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
68
1/2
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
 39 x.head(state_->get_nq());
69 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
70
1/2
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 39 data->Rx.leftCols(state_->get_nv());
71 39 pinocchio::computeStaticTorqueDerivatives(pin_model_, d->pinocchio, q,
72
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 d->fext, Rq);
73
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 Rq *= -1;
74
1/2
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
 39 data->Ru = d->actuation->dtau_du;
75 39 }
76
77 template <typename Scalar>
78 39 void ResidualModelContactControlGravTpl<Scalar>::calcDiff(
79 const boost::shared_ptr<ResidualDataAbstract> &data,
80 const Eigen::Ref<const VectorXs> &x) {
81 39 Data *d = static_cast<Data *>(data.get());
82
83 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
84
1/2
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
 39 x.head(state_->get_nq());
85 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
86
1/2
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 39 data->Rx.leftCols(state_->get_nv());
87
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q,
88 Rq);
89
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 Rq *= -1;
90 39 }
91
92 template <typename Scalar>
93 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
94 2774 ResidualModelContactControlGravTpl<Scalar>::createData(
95 DataCollectorAbstract *const data) {
96 ✗ return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
97
1/2
✓ Branch 2 taken 2774 times.
✗ Branch 3 not taken.
 2774 data);
98 }
99
100 template <typename Scalar>
101 ✗ void ResidualModelContactControlGravTpl<Scalar>::print(std::ostream &os) const {
102 ✗ os << "ResidualModelContactControlGrav";
103 }
104
105 } // namespace crocoddyl
106