GCC Code Coverage Report

Directory: ./
File: src/kinodynamic-oriented-path.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 44 63 69.8%
Branches: 58 164 35.4%
Line Branch Exec Source
1 // Copyright (c) 2016, LAAS-CNRS
2 // Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/core/kinodynamic-oriented-path.hh>
30 #include <hpp/pinocchio/device.hh>
31
32 namespace hpp {
33 namespace core {
34
35 11000 void KinodynamicOrientedPath::orienteInitAndGoal(const DevicePtr_t& device) {
36 // adjust initial and end configs, with correct orientation (aligned with the
37 // velocity):
38 size_type configSize =
39
1/2
✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
 11000 device->configSize() - device->extraConfigSpace().dimension();
40
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 Eigen::Vector3d vi(initial_[configSize], initial_[configSize + 1],
41
3/6
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11000 times.
✗ Branch 8 not taken.
 22000 initial_[configSize + 2]);
42
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 Eigen::Vector3d ve(end_[configSize], end_[configSize + 1],
43
3/6
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11000 times.
✗ Branch 8 not taken.
 22000 end_[configSize + 2]);
44 hppDout(notice, "OrienteInitAndGoal: ignoreZValue = " << ignoreZValue_);
45
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 11000 times.
 11000 if (ignoreZValue_) {
46 ✗ vi[2] = 0.;
47 ✗ ve[2] = 0.;
48 }
49
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11000 times.
✗ Branch 4 not taken.
 11000 if (vi.norm() > 0) { // if velocity in the state
50 Eigen::Quaterniond quat =
51
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), vi);
52
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 initial_[6] = quat.w();
53
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 initial_[3] = quat.x();
54
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 initial_[4] = quat.y();
55
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 initial_[5] = quat.z();
56 }
57
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11000 times.
✗ Branch 4 not taken.
 11000 if (ve.norm() > 0) { // if velocity in the state
58 Eigen::Quaterniond quat =
59
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), ve);
60
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 end_[6] = quat.w();
61
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 end_[3] = quat.x();
62
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 end_[4] = quat.y();
63
2/4
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11000 times.
✗ Branch 5 not taken.
 11000 end_[5] = quat.z();
64 }
65 11000 }
66
67 ✗ KinodynamicOrientedPath::KinodynamicOrientedPath(
68 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
69 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
70 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
71 ✗ ConfigurationIn_t vLim, bool ignoreZValue)
72 : parent_t(device, init, end, length, a1, t0, t1, tv, t2, vLim),
73 ✗ ignoreZValue_(ignoreZValue) {
74 ✗ orienteInitAndGoal(device);
75 }
76
77 ✗ KinodynamicOrientedPath::KinodynamicOrientedPath(
78 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
79 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
80 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
81 ✗ ConfigurationIn_t vLim, ConstraintSetPtr_t constraints, bool ignoreZValue)
82 : parent_t(device, init, end, length, a1, t0, t1, tv, t2, vLim,
83 constraints),
84 ✗ ignoreZValue_(ignoreZValue)
85
86 {
87 ✗ orienteInitAndGoal(device);
88 }
89
90 ✗ KinodynamicOrientedPath::KinodynamicOrientedPath(
91 ✗ const KinodynamicOrientedPath& path)
92 ✗ : parent_t(path), ignoreZValue_(path.ignoreZValue()) {
93 hppDout(notice, "Create copy called, ignoreZValue = " << ignoreZValue_);
94 ✗ orienteInitAndGoal(path.device());
95 }
96
97 11000 KinodynamicOrientedPath::KinodynamicOrientedPath(const KinodynamicPath& path,
98 11000 bool ignoreZValue)
99 11000 : parent_t(path), ignoreZValue_(ignoreZValue) {
100
1/2
✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
 11000 orienteInitAndGoal(device());
101 11000 }
102
103 ✗ KinodynamicOrientedPath::KinodynamicOrientedPath(
104 ✗ const KinodynamicOrientedPath& path, const ConstraintSetPtr_t& constraints)
105 ✗ : parent_t(path, constraints), ignoreZValue_(path.ignoreZValue()) {
106 ✗ orienteInitAndGoal(device());
107 }
108
109 82000 bool KinodynamicOrientedPath::impl_compute(ConfigurationOut_t result,
110 value_type t) const {
111
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 parent_t::impl_compute(result, t);
112 // Re oriente the robot such that he always face the velocity vector
113 // FIX ME : assume first joint is freeflyer
114 size_type configSize =
115
1/2
✓ Branch 3 taken 82000 times.
✗ Branch 4 not taken.
 82000 device()->configSize() - device()->extraConfigSpace().dimension();
116
1/2
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
 82000 Eigen::Vector3d v(result[configSize], result[configSize + 1],
117
3/6
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 82000 times.
✗ Branch 8 not taken.
 164000 result[configSize + 2]);
118
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 82000 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 82000 if (ignoreZValue_) v[2] = 0;
119
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 82000 times.
✗ Branch 4 not taken.
 82000 if (v.norm() > 0) { // if velocity in the state
120 Eigen::Quaterniond quat =
121
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), v);
122
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 result[6] = quat.w();
123
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 result[3] = quat.x();
124
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 result[4] = quat.y();
125
2/4
✓ Branch 1 taken 82000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82000 times.
✗ Branch 5 not taken.
 82000 result[5] = quat.z();
126 }
127
128 82000 return true;
129 }
130
131 10000 PathPtr_t KinodynamicOrientedPath::impl_extract(
132 const interval_t& subInterval) const {
133
1/2
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
 10000 PathPtr_t path = parent_t::impl_extract(subInterval);
134 10000 KinodynamicPathPtr_t kinoPath = dynamic_pointer_cast<KinodynamicPath>(path);
135
1/2
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
 10000 if (kinoPath)
136
1/2
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
 10000 return KinodynamicOrientedPath::create(kinoPath, ignoreZValue_);
137 else {
138 hppDout(error,
139 "Error while casting path in KinodynamicOrientedPath::extract");
140 ✗ return PathPtr_t();
141 }
142 10000 }
143
144 } // namespace core
145 } // namespace hpp
146