GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/costs/residual.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 39 50 78.0%
Branches: 13 42 31.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, INRIA,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 namespace crocoddyl {
11 template <typename Scalar>
12 5362 CostModelResidualTpl<Scalar>::CostModelResidualTpl(
13 std::shared_ptr<typename Base::StateAbstract> state,
14 std::shared_ptr<ActivationModelAbstract> activation,
15 std::shared_ptr<ResidualModelAbstract> residual)
16
1/2
✓ Branch 4 taken 5362 times.
✗ Branch 5 not taken.
 5362 : Base(state, activation, residual) {}
17
18 template <typename Scalar>
19 1673 CostModelResidualTpl<Scalar>::CostModelResidualTpl(
20 std::shared_ptr<typename Base::StateAbstract> state,
21 std::shared_ptr<ResidualModelAbstract> residual)
22
1/2
✓ Branch 3 taken 1673 times.
✗ Branch 4 not taken.
 1673 : Base(state, residual) {}
23
24 template <typename Scalar>
25 375232 void CostModelResidualTpl<Scalar>::calc(
26 const std::shared_ptr<CostDataAbstract>& data,
27 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28 // Compute the cost residual
29 375232 residual_->calc(data->residual, x, u);
30
31 // Compute the cost
32
1/2
✓ Branch 6 taken 375232 times.
✗ Branch 7 not taken.
 375232 activation_->calc(data->activation, data->residual->r);
33 375232 data->cost = data->activation->a_value;
34 375232 }
35
36 template <typename Scalar>
37 83143 void CostModelResidualTpl<Scalar>::calc(
38 const std::shared_ptr<CostDataAbstract>& data,
39 const Eigen::Ref<const VectorXs>& x) {
40 83143 const bool is_rq = residual_->get_q_dependent();
41 83143 const bool is_rv = residual_->get_v_dependent();
42
3/4
✓ Branch 0 taken 24011 times.
✓ Branch 1 taken 59132 times.
✓ Branch 2 taken 24011 times.
✗ Branch 3 not taken.
 83143 if (!is_rq && !is_rv) {
43 24011 data->activation->a_value = 0.;
44 24011 data->cost = 0.;
45 24011 return; // do nothing
46 }
47
48 // Compute the cost residual
49 59132 residual_->calc(data->residual, x);
50
51 // Compute the cost
52
1/2
✓ Branch 6 taken 59132 times.
✗ Branch 7 not taken.
 59132 activation_->calc(data->activation, data->residual->r);
53 59132 data->cost = data->activation->a_value;
54 }
55
56 template <typename Scalar>
57 60564 void CostModelResidualTpl<Scalar>::calcDiff(
58 const std::shared_ptr<CostDataAbstract>& data,
59 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
60 // Compute the derivatives of the activation and contact wrench cone residual
61 // models
62 60564 residual_->calcDiff(data->residual, x, u);
63
1/2
✓ Branch 6 taken 60564 times.
✗ Branch 7 not taken.
 60564 activation_->calcDiff(data->activation, data->residual->r);
64
65 // Compute the derivatives of the cost function based on a Gauss-Newton
66 // approximation
67 60564 residual_->calcCostDiff(data, data->residual, data->activation);
68 60564 }
69
70 template <typename Scalar>
71 3207 void CostModelResidualTpl<Scalar>::calcDiff(
72 const std::shared_ptr<CostDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>& x) {
74 // Compute the derivatives of the activation and contact wrench cone residual
75 // models
76 3207 const bool is_rq = residual_->get_q_dependent();
77 3207 const bool is_rv = residual_->get_v_dependent();
78
3/4
✓ Branch 0 taken 1181 times.
✓ Branch 1 taken 2026 times.
✓ Branch 2 taken 1181 times.
✗ Branch 3 not taken.
 3207 if (!is_rq && !is_rv) {
79 1181 data->Lx.setZero();
80 1181 data->Lxx.setZero();
81 1181 return; // do nothing
82 }
83 2026 residual_->calcDiff(data->residual, x);
84
1/2
✓ Branch 6 taken 2026 times.
✗ Branch 7 not taken.
 2026 activation_->calcDiff(data->activation, data->residual->r);
85
86 // Compute the derivatives of the cost function based on a Gauss-Newton
87 // approximation
88 2026 residual_->calcCostDiff(data, data->residual, data->activation, false);
89 }
90
91 template <typename Scalar>
92 std::shared_ptr<CostDataAbstractTpl<Scalar> >
93 475650 CostModelResidualTpl<Scalar>::createData(DataCollectorAbstract* const data) {
94 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
95
1/2
✓ Branch 2 taken 475650 times.
✗ Branch 3 not taken.
 475650 data);
96 }
97
98 template <typename Scalar>
99 template <typename NewScalar>
100 ✗ CostModelResidualTpl<NewScalar> CostModelResidualTpl<Scalar>::cast() const {
101 typedef CostModelResidualTpl<NewScalar> ReturnType;
102 ✗ if (activation_) {
103 ✗ ReturnType ret(state_->template cast<NewScalar>(),
104 ✗ activation_->template cast<NewScalar>(),
105 ✗ residual_->template cast<NewScalar>());
106 ✗ return ret;
107 ✗ } else {
108 ✗ ReturnType ret(state_->template cast<NewScalar>(),
109 ✗ residual_->template cast<NewScalar>());
110 ✗ return ret;
111 }
112 }
113
114 template <typename Scalar>
115 324 void CostModelResidualTpl<Scalar>::print(std::ostream& os) const {
116 324 os << "CostModelResidual {" << *residual_ << ", " << *activation_ << "}";
117 324 }
118
119 } // namespace crocoddyl
120