GCC Code Coverage Report

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