GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actuation/actuation-squashing.hpp
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 32 33 97.0%
Branches: 10 36 27.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, University of Edinburgh, IRI: CSIC-UPC
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
10 #define CROCODDYL_CORE_ACTUATION_SQUASHING_HPP_
11
12 #include "crocoddyl/core/actuation-base.hpp"
13 #include "crocoddyl/core/actuation/squashing-base.hpp"
14 #include "crocoddyl/core/fwd.hpp"
15
16 namespace crocoddyl {
17
18 template <typename _Scalar>
19 class ActuationSquashingModelTpl : public ActuationModelAbstractTpl<_Scalar> {
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22
23 typedef _Scalar Scalar;
24 typedef MathBaseTpl<Scalar> MathBase;
25 typedef ActuationModelAbstractTpl<Scalar> Base;
26 typedef ActuationSquashingDataTpl<Scalar> Data;
27 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
28 typedef ActuationDataAbstractTpl<Scalar> ActuationDataAbstract;
29 typedef SquashingModelAbstractTpl<Scalar> SquashingModelAbstract;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
32
33 280 ActuationSquashingModelTpl(std::shared_ptr<ActuationModelAbstract> actuation,
34 std::shared_ptr<SquashingModelAbstract> squashing,
35 const std::size_t nu)
36 : Base(actuation->get_state(), nu),
37 280 squashing_(squashing),
38 280 actuation_(actuation) {};
39
40 564 virtual ~ActuationSquashingModelTpl() {};
41
42 14098 virtual void calc(const std::shared_ptr<ActuationDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& x,
44 const Eigen::Ref<const VectorXs>& u) {
45 14098 Data* d = static_cast<Data*>(data.get());
46
47 14098 squashing_->calc(d->squashing, u);
48
1/2
✓ Branch 4 taken 14098 times.
✗ Branch 5 not taken.
 14098 actuation_->calc(d->actuation, x, d->squashing->u);
49 14098 data->tau = d->actuation->tau;
50 14098 data->tau_set = d->actuation->tau_set;
51 14098 };
52
53 4691 virtual void calcDiff(const std::shared_ptr<ActuationDataAbstract>& data,
54 const Eigen::Ref<const VectorXs>& x,
55 const Eigen::Ref<const VectorXs>& u) {
56 4691 Data* d = static_cast<Data*>(data.get());
57
58 4691 squashing_->calcDiff(d->squashing, u);
59
1/2
✓ Branch 4 taken 4691 times.
✗ Branch 5 not taken.
 4691 actuation_->calcDiff(d->actuation, x, d->squashing->u);
60
2/4
✓ Branch 5 taken 4691 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4691 times.
✗ Branch 9 not taken.
 4691 data->dtau_du.noalias() = d->actuation->dtau_du * d->squashing->du_ds;
61 4691 };
62
63 2425 virtual void commands(const std::shared_ptr<ActuationDataAbstract>& data,
64 const Eigen::Ref<const VectorXs>& x,
65 const Eigen::Ref<const VectorXs>& tau) {
66
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2425 times.
 2425 if (static_cast<std::size_t>(tau.size()) != this->state_->get_nv()) {
67 ✗ throw_pretty("Invalid argument: "
68 << "tau has wrong dimension (it should be " +
69 std::to_string(this->state_->get_nv()) + ")");
70 }
71 2425 this->torqueTransform(data, x, tau);
72
2/4
✓ Branch 4 taken 2425 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2425 times.
✗ Branch 8 not taken.
 2425 data->u.noalias() = data->Mtau * tau;
73 2425 }
74
75 8089 std::shared_ptr<ActuationDataAbstract> createData() {
76
1/2
✓ Branch 2 taken 8089 times.
✗ Branch 3 not taken.
 8089 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
77 };
78
79 8089 const std::shared_ptr<SquashingModelAbstract>& get_squashing() const {
80 8089 return squashing_;
81 };
82 8089 const std::shared_ptr<ActuationModelAbstract>& get_actuation() const {
83 8089 return actuation_;
84 };
85
86 protected:
87 std::shared_ptr<SquashingModelAbstract> squashing_;
88 std::shared_ptr<ActuationModelAbstract> actuation_;
89 };
90
91 template <typename _Scalar>
92 struct ActuationSquashingDataTpl : public ActuationDataAbstractTpl<_Scalar> {
93 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94
95 typedef _Scalar Scalar;
96 typedef ActuationDataAbstractTpl<Scalar> Base;
97 typedef SquashingDataAbstractTpl<Scalar> SquashingDataAbstract;
98 typedef MathBaseTpl<Scalar> MathBase;
99 typedef typename MathBase::VectorXs VectorXs;
100 typedef typename MathBase::MatrixXs MatrixXs;
101
102 template <template <typename Scalar> class Model>
103 8089 explicit ActuationSquashingDataTpl(Model<Scalar>* const model)
104 : Base(model),
105
1/2
✓ Branch 2 taken 8089 times.
✗ Branch 3 not taken.
 8089 squashing(model->get_squashing()->createData()),
106
1/2
✓ Branch 5 taken 8089 times.
✗ Branch 6 not taken.
 16178 actuation(model->get_actuation()->createData()) {}
107
108 16182 ~ActuationSquashingDataTpl() {}
109
110 std::shared_ptr<SquashingDataAbstract> squashing;
111 std::shared_ptr<ActuationDataAbstract> actuation;
112
113 using Base::dtau_du;
114 using Base::dtau_dx;
115 using Base::tau;
116 using Base::tau_set;
117 };
118
119 } // namespace crocoddyl
120
121 #endif // CROCODDYL_CORE_ACTIVATION_SQUASH_BASE_HPP_
122