GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residuals/joint-effort.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 32 52 61.5%
Branches: 17 84 20.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022, Heriot-Watt University, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/residuals/joint-effort.hpp"
10 #include "crocoddyl/core/utils/exception.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 597 ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
16 boost::shared_ptr<StateAbstract> state,
17 boost::shared_ptr<ActuationModelAbstract> actuation, const VectorXs& uref,
18 const std::size_t nu, const bool fwddyn)
19 : Base(state, actuation->get_nu(), nu, fwddyn ? false : true,
20 fwddyn ? false : true, true),
21 597 uref_(uref),
22
4/8
✓ Branch 0 taken 597 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 597 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 597 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 597 times.
✗ Branch 13 not taken.
 597 fwddyn_(fwddyn) {
23
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 597 times.
 597 if (nu_ == 0) {
24 ✗ throw_pretty("Invalid argument: "
25 << "it seems to be an autonomous system, if so, don't add "
26 "this residual function");
27 }
28 597 }
29
30 template <typename Scalar>
31 ✗ ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
32 boost::shared_ptr<StateAbstract> state,
33 boost::shared_ptr<ActuationModelAbstract> actuation, const VectorXs& uref)
34 : Base(state, actuation->get_nu(), state->get_nv(), true, true, true),
35 ✗ uref_(uref),
36 ✗ fwddyn_(false) {}
37
38 template <typename Scalar>
39 1 ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
40 boost::shared_ptr<StateAbstract> state,
41 boost::shared_ptr<ActuationModelAbstract> actuation, const std::size_t nu)
42 : Base(state, actuation->get_nu(), nu, true, true, true),
43
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 uref_(VectorXs::Zero(actuation->get_nu())),
44
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 fwddyn_(false) {
45
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (nu_ == 0) {
46 ✗ throw_pretty("Invalid argument: "
47 << "it seems to be an autonomous system, if so, don't add "
48 "this residual function");
49 }
50 1 }
51
52 template <typename Scalar>
53 1 ResidualModelJointEffortTpl<Scalar>::ResidualModelJointEffortTpl(
54 boost::shared_ptr<StateAbstract> state,
55 boost::shared_ptr<ActuationModelAbstract> actuation)
56 : Base(state, actuation->get_nu(), state->get_nv(), true, true, true),
57
3/6
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 uref_(VectorXs::Zero(actuation->get_nu())) {}
58
59 template <typename Scalar>
60 1202 ResidualModelJointEffortTpl<Scalar>::~ResidualModelJointEffortTpl() {}
61
62 template <typename Scalar>
63 38100 void ResidualModelJointEffortTpl<Scalar>::calc(
64 const boost::shared_ptr<ResidualDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
66 38100 Data* d = static_cast<Data*>(data.get());
67
1/2
✓ Branch 4 taken 38100 times.
✗ Branch 5 not taken.
 38100 data->r = d->joint->tau - uref_;
68 38100 }
69
70 template <typename Scalar>
71 ✗ void ResidualModelJointEffortTpl<Scalar>::calc(
72 const boost::shared_ptr<ResidualDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>&) {
74 ✗ if (fwddyn_) {
75 ✗ data->r.setZero();
76 } else {
77 ✗ Data* d = static_cast<Data*>(data.get());
78 ✗ data->r = d->joint->tau - uref_;
79 }
80 }
81
82 template <typename Scalar>
83 6945 void ResidualModelJointEffortTpl<Scalar>::calcDiff(
84 const boost::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
86 6945 Data* d = static_cast<Data*>(data.get());
87
2/4
✓ Branch 0 taken 6945 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 6945 times.
 6945 if (q_dependent_ || v_dependent_) {
88 ✗ data->Rx = d->joint->dtau_dx;
89 }
90 6945 data->Ru = d->joint->dtau_du;
91 6945 }
92
93 template <typename Scalar>
94 ✗ void ResidualModelJointEffortTpl<Scalar>::calcDiff(
95 const boost::shared_ptr<ResidualDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>&) {
97 ✗ if (fwddyn_) {
98 ✗ data->Rx.setZero();
99 } else {
100 ✗ Data* d = static_cast<Data*>(data.get());
101 ✗ data->Rx = d->joint->dtau_dx;
102 ✗ data->Ru = d->joint->dtau_du;
103 }
104 }
105
106 template <typename Scalar>
107 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
108 48185 ResidualModelJointEffortTpl<Scalar>::createData(
109 DataCollectorAbstract* const data) {
110
1/2
✓ Branch 1 taken 48185 times.
✗ Branch 2 not taken.
 96370 boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
111 48185 Eigen::aligned_allocator<Data>(), this, data);
112 48185 return d;
113 }
114
115 template <typename Scalar>
116 ✗ void ResidualModelJointEffortTpl<Scalar>::print(std::ostream& os) const {
117 ✗ os << "ResidualModelJointEffort";
118 }
119
120 template <typename Scalar>
121 const typename MathBaseTpl<Scalar>::VectorXs&
122 1 ResidualModelJointEffortTpl<Scalar>::get_reference() const {
123 1 return uref_;
124 }
125
126 template <typename Scalar>
127 1 void ResidualModelJointEffortTpl<Scalar>::set_reference(
128 const VectorXs& reference) {
129
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (static_cast<std::size_t>(reference.size()) != nr_) {
130 ✗ throw_pretty("Invalid argument: "
131 << "the joint-effort reference has wrong dimension ("
132 << reference.size()
133 << " provided - it should be " + std::to_string(nr_) + ")")
134 }
135 1 uref_ = reference;
136 1 }
137
138 } // namespace crocoddyl
139