GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actions/unicycle.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 59 77 76.6%
Branches: 56 218 25.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, 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/utils/exception.hpp"
11
12 namespace crocoddyl {
13 template <typename Scalar>
14 33 ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
15 : ActionModelAbstractTpl<Scalar>(
16 ✗ std::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
17
3/6
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 33 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 33 times.
✗ Branch 11 not taken.
 33 dt_(Scalar(0.1)) {
18
2/4
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
 33 cost_weights_ << Scalar(10.), Scalar(1.);
19 33 }
20
21 template <typename Scalar>
22 70 ActionModelUnicycleTpl<Scalar>::~ActionModelUnicycleTpl() {}
23
24 template <typename Scalar>
25 2428 void ActionModelUnicycleTpl<Scalar>::calc(
26 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
27 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2428 times.
 2428 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
29 ✗ throw_pretty(
30 "Invalid argument: " << "x has wrong dimension (it should be " +
31 std::to_string(state_->get_nx()) + ")");
32 }
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2428 times.
 2428 if (static_cast<std::size_t>(u.size()) != nu_) {
34 ✗ throw_pretty(
35 "Invalid argument: " << "u has wrong dimension (it should be " +
36 std::to_string(nu_) + ")");
37 }
38 2428 Data* d = static_cast<Data*>(data.get());
39
40 2428 const Scalar c = cos(x[2]);
41 2428 const Scalar s = sin(x[2]);
42
7/14
✓ Branch 3 taken 2428 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2428 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2428 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2428 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2428 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2428 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2428 times.
✗ Branch 22 not taken.
 2428 d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
43
2/4
✓ Branch 3 taken 2428 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2428 times.
✗ Branch 7 not taken.
 2428 d->r.template head<3>() = cost_weights_[0] * x;
44
2/4
✓ Branch 3 taken 2428 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2428 times.
✗ Branch 7 not taken.
 2428 d->r.template tail<2>() = cost_weights_[1] * u;
45 2428 d->cost = Scalar(0.5) * d->r.dot(d->r);
46 2428 }
47
48 template <typename Scalar>
49 187 void ActionModelUnicycleTpl<Scalar>::calc(
50 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
51 const Eigen::Ref<const VectorXs>& x) {
52
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 187 times.
 187 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
53 ✗ throw_pretty(
54 "Invalid argument: " << "x has wrong dimension (it should be " +
55 std::to_string(state_->get_nx()) + ")");
56 }
57 187 Data* d = static_cast<Data*>(data.get());
58
59 187 d->xnext = x;
60
2/4
✓ Branch 3 taken 187 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 187 times.
✗ Branch 7 not taken.
 187 d->r.template head<3>() = cost_weights_[0] * x;
61
1/2
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
 187 d->r.template tail<2>().setZero();
62
2/4
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 187 times.
✗ Branch 6 not taken.
 187 d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
63 187 }
64
65 template <typename Scalar>
66 1474 void ActionModelUnicycleTpl<Scalar>::calcDiff(
67 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
68 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
69
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1474 times.
 1474 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
70 ✗ throw_pretty(
71 "Invalid argument: " << "x has wrong dimension (it should be " +
72 std::to_string(state_->get_nx()) + ")");
73 }
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1474 times.
 1474 if (static_cast<std::size_t>(u.size()) != nu_) {
75 ✗ throw_pretty(
76 "Invalid argument: " << "u has wrong dimension (it should be " +
77 std::to_string(nu_) + ")");
78 }
79 1474 Data* d = static_cast<Data*>(data.get());
80
81
1/2
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
 1474 const Scalar c = cos(x[2]);
82
1/2
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
 1474 const Scalar s = sin(x[2]);
83
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 const Scalar w_x = cost_weights_[0] * cost_weights_[0];
84
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 const Scalar w_u = cost_weights_[1] * cost_weights_[1];
85
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Lx = x * w_x;
86
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Lu = u * w_u;
87
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Lxx.diagonal().setConstant(w_x);
88
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Luu.diagonal().setConstant(w_u);
89
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Fx(0, 2) = -s * u[0] * dt_;
90
2/4
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1474 times.
✗ Branch 5 not taken.
 1474 d->Fx(1, 2) = c * u[0] * dt_;
91
1/2
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
 1474 d->Fu(0, 0) = c * dt_;
92
1/2
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
 1474 d->Fu(1, 0) = s * dt_;
93
1/2
✓ Branch 1 taken 1474 times.
✗ Branch 2 not taken.
 1474 d->Fu(2, 1) = dt_;
94 1474 }
95
96 template <typename Scalar>
97 111 void ActionModelUnicycleTpl<Scalar>::calcDiff(
98 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
99 const Eigen::Ref<const VectorXs>& x) {
100
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 111 times.
 111 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
101 ✗ throw_pretty(
102 "Invalid argument: " << "x has wrong dimension (it should be " +
103 std::to_string(state_->get_nx()) + ")");
104 }
105 111 Data* d = static_cast<Data*>(data.get());
106
107
2/4
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111 times.
✗ Branch 5 not taken.
 111 const Scalar w_x = cost_weights_[0] * cost_weights_[0];
108
2/4
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111 times.
✗ Branch 5 not taken.
 111 d->Lx = x * w_x;
109
2/4
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111 times.
✗ Branch 5 not taken.
 111 d->Lxx.diagonal().setConstant(w_x);
110 111 }
111
112 template <typename Scalar>
113 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
114 948 ActionModelUnicycleTpl<Scalar>::createData() {
115
1/2
✓ Branch 2 taken 948 times.
✗ Branch 3 not taken.
 948 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
116 }
117
118 template <typename Scalar>
119 64 bool ActionModelUnicycleTpl<Scalar>::checkData(
120 const std::shared_ptr<ActionDataAbstract>& data) {
121 64 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
122
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 if (d != NULL) {
123 64 return true;
124 } else {
125 ✗ return false;
126 }
127 64 }
128
129 template <typename Scalar>
130 22 void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
131 22 os << "ActionModelUnicycle {dt=" << dt_ << "}";
132 22 }
133
134 template <typename Scalar>
135 const typename MathBaseTpl<Scalar>::Vector2s&
136 ✗ ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
137 ✗ return cost_weights_;
138 }
139
140 template <typename Scalar>
141 ✗ void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
142 const typename MathBase::Vector2s& weights) {
143 ✗ cost_weights_ = weights;
144 }
145
146 template <typename Scalar>
147 ✗ Scalar ActionModelUnicycleTpl<Scalar>::get_dt() const {
148 ✗ return dt_;
149 }
150
151 template <typename Scalar>
152 ✗ void ActionModelUnicycleTpl<Scalar>::set_dt(const Scalar dt) {
153 ✗ if (dt <= 0)
154 ✗ throw_pretty("Invalid argument: dt should be strictly positive.");
155 ✗ dt_ = dt;
156 }
157
158 } // namespace crocoddyl
159