GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/control.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 41 45 91.1%
Branches: 19 102 18.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, 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 #include "crocoddyl/core/residuals/control.hpp"
11 #include "crocoddyl/core/utils/exception.hpp"
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 1411 ResidualModelControlTpl<Scalar>::ResidualModelControlTpl(
17 boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& uref)
18 1411 : Base(state, static_cast<std::size_t>(uref.size()),
19 1411 static_cast<std::size_t>(uref.size()), false, false, true),
20
2/4
✓ Branch 4 taken 1411 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1411 times.
✗ Branch 9 not taken.
 1411 uref_(uref) {
21
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1411 times.
 1411 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 1411 }
27
28 template <typename Scalar>
29 4 ResidualModelControlTpl<Scalar>::ResidualModelControlTpl(
30 boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu)
31
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 4 : Base(state, nu, nu, false, false, true), uref_(VectorXs::Zero(nu)) {
32
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (nu_ == 0) {
33 ✗ throw_pretty("Invalid argument: "
34 << "it seems to be an autonomous system, if so, don't add "
35 "this residual function");
36 }
37 4 }
38
39 template <typename Scalar>
40 12 ResidualModelControlTpl<Scalar>::ResidualModelControlTpl(
41 boost::shared_ptr<typename Base::StateAbstract> state)
42 : Base(state, state->get_nv(), state->get_nv(), false, false, true),
43
3/6
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 12 times.
✗ Branch 16 not taken.
 12 uref_(VectorXs::Zero(state->get_nv())) {}
44
45 template <typename Scalar>
46 2858 ResidualModelControlTpl<Scalar>::~ResidualModelControlTpl() {}
47
48 template <typename Scalar>
49 74903 void ResidualModelControlTpl<Scalar>::calc(
50 const boost::shared_ptr<ResidualDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>& u) {
52
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 74903 times.
 74903 if (static_cast<std::size_t>(u.size()) != nu_) {
53 ✗ throw_pretty(
54 "Invalid argument: " << "u has wrong dimension (it should be " +
55 std::to_string(nu_) + ")");
56 }
57
58
1/2
✓ Branch 3 taken 74903 times.
✗ Branch 4 not taken.
 74903 data->r = u - uref_;
59 74903 }
60
61 template <typename Scalar>
62 1252 void ResidualModelControlTpl<Scalar>::calc(
63 const boost::shared_ptr<ResidualDataAbstract>& data,
64 const Eigen::Ref<const VectorXs>&) {
65 1252 data->r.setZero();
66 1252 }
67
68 template <typename Scalar>
69 #ifndef NDEBUG
70 13765 void ResidualModelControlTpl<Scalar>::calcDiff(
71 const boost::shared_ptr<ResidualDataAbstract>& data,
72 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
73 #else
74 void ResidualModelControlTpl<Scalar>::calcDiff(
75 const boost::shared_ptr<ResidualDataAbstract>&,
76 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
77 #endif
78 // The Jacobian has constant values which were set in createData.
79
3/14
✓ Branch 4 taken 13765 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13765 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 13765 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 13765 assert_pretty(MatrixXs(data->Ru).isApprox(MatrixXs::Identity(nu_, nu_)),
80 "Ru has wrong value");
81 13765 }
82
83 template <typename Scalar>
84 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
85 96945 ResidualModelControlTpl<Scalar>::createData(
86 DataCollectorAbstract* const _data) {
87
1/2
✓ Branch 1 taken 96945 times.
✗ Branch 2 not taken.
 96945 boost::shared_ptr<ResidualDataAbstract> data =
88 boost::allocate_shared<ResidualDataAbstract>(
89 96945 Eigen::aligned_allocator<ResidualDataAbstract>(), this, _data);
90
2/4
✓ Branch 2 taken 96945 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96945 times.
✗ Branch 6 not taken.
 96945 data->Ru.diagonal().fill((Scalar)1.);
91 96945 return data;
92 }
93
94 template <typename Scalar>
95 13683 void ResidualModelControlTpl<Scalar>::calcCostDiff(
96 const boost::shared_ptr<CostDataAbstract>& cdata,
97 const boost::shared_ptr<ResidualDataAbstract>&,
98 const boost::shared_ptr<ActivationDataAbstract>& adata, const bool) {
99 13683 cdata->Lu = adata->Ar;
100 13683 cdata->Luu = adata->Arr;
101 13683 }
102
103 template <typename Scalar>
104 63 void ResidualModelControlTpl<Scalar>::print(std::ostream& os) const {
105 63 os << "ResidualModelControl";
106 63 }
107
108 template <typename Scalar>
109 const typename MathBaseTpl<Scalar>::VectorXs&
110 1 ResidualModelControlTpl<Scalar>::get_reference() const {
111 1 return uref_;
112 }
113
114 template <typename Scalar>
115 1 void ResidualModelControlTpl<Scalar>::set_reference(const VectorXs& reference) {
116
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (static_cast<std::size_t>(reference.size()) != nu_) {
117 ✗ throw_pretty("Invalid argument: "
118 << "the control reference has wrong dimension ("
119 << reference.size()
120 << " provided - it should be " + std::to_string(nu_) + ")")
121 }
122 1 uref_ = reference;
123 1 }
124
125 } // namespace crocoddyl
126