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 |