GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// Copyright (c) 2014, LAAS-CNRS |
||
2 |
// Authors: Joseph Mirabel (joseph.mirabel@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/manipulation/srdf/factories/position.hh" |
||
30 |
|||
31 |
#include <hpp/util/debug.hh> |
||
32 |
#include <pinocchio/spatial/se3.hpp> |
||
33 |
|||
34 |
namespace hpp { |
||
35 |
namespace manipulation { |
||
36 |
namespace srdf { |
||
37 |
typedef Eigen::Quaternion<value_type> Quaternion_t; |
||
38 |
|||
39 |
7 |
void PositionFactory::finishTags() { |
|
40 |
✓✓ | 7 |
if (values().size() != 0) |
41 |
3 |
computeTransformFromText(); |
|
42 |
else |
||
43 |
4 |
computeTransformFromAttributes(); |
|
44 |
7 |
} |
|
45 |
|||
46 |
3 |
void PositionFactory::computeTransformFromText() { |
|
47 |
3 |
const std::vector<float>& v = values(); |
|
48 |
✓✗ | 3 |
Quaternion_t q(v[3], v[4], v[5], v[6]); // w, x, y, z |
49 |
✓✗ | 3 |
if (std::fabs(1 - q.squaredNorm()) < 1e-4) { |
50 |
hppDout(warning, "Quaternion is not normalized."); |
||
51 |
} |
||
52 |
✓✗ | 3 |
q.normalize(); |
53 |
✓✗✓✗ ✓✗ |
3 |
position_ = Transform3f(q.matrix(), vector3_t(v[0], v[1], v[2])); |
54 |
3 |
} |
|
55 |
|||
56 |
4 |
void PositionFactory::computeTransformFromAttributes() { |
|
57 |
✓✗ | 4 |
vector3_t xyz(0, 0, 0); |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
if (hasAttribute("xyz")) parser::readSequence(getAttribute("xyz"), xyz, 3); |
59 |
|||
60 |
✓✗✓✗ |
4 |
bool a_rpy = hasAttribute("rpy"); |
61 |
✓✗✓✗ |
4 |
bool a_wxyz = hasAttribute("wxyz"); |
62 |
✓✗✓✗ |
4 |
bool a_xyzw = hasAttribute("xyzw"); |
63 |
✗✓ | 4 |
if ((int)a_rpy + (int)a_wxyz + (int)a_xyzw > 1) |
64 |
throw std::invalid_argument("Tag " + tagName() + |
||
65 |
" must have only one of rpy, wxyz, xyzw"); |
||
66 |
|||
67 |
✓✗✓✗ |
4 |
matrix3_t R(matrix3_t::Identity()); |
68 |
✓✓ | 4 |
if (a_rpy) { |
69 |
✓✗ | 1 |
vector3_t rpy; |
70 |
✓✗✓✗ ✓✗ |
1 |
parser::readSequence(getAttribute("rpy"), rpy, 3); |
71 |
|||
72 |
typedef Eigen::AngleAxis<value_type> AngleAxis; |
||
73 |
✓✗✓✗ ✓✗✓✗ |
1 |
R = AngleAxis(rpy[2], vector3_t::UnitZ()) * |
74 |
✓✗✓✗ ✓✗✓✗ |
2 |
AngleAxis(rpy[1], vector3_t::UnitY()) * |
75 |
✓✗✓✗ ✓✗✓✗ |
2 |
AngleAxis(rpy[0], vector3_t::UnitX()); |
76 |
✓✓✓✓ |
3 |
} else if (a_wxyz || a_xyzw) { |
77 |
✓✗ | 2 |
Quaternion_t q; |
78 |
|||
79 |
✓✓ | 2 |
if (a_wxyz) { |
80 |
✓✗ | 1 |
vector_t wxyz(4); |
81 |
✓✗✓✗ ✓✗ |
1 |
parser::readSequence(getAttribute("wxyz"), wxyz, 4); |
82 |
✓✗✓✗ |
1 |
q.w() = wxyz[0]; |
83 |
✓✗✓✗ ✓✗ |
1 |
q.vec() = wxyz.tail<3>(); |
84 |
✓✗ | 1 |
} else if (a_xyzw) { |
85 |
✓✗ | 1 |
vector_t xyzw(4); |
86 |
✓✗✓✗ ✓✗ |
1 |
parser::readSequence(getAttribute("xyzw"), xyzw, 4); |
87 |
✓✗✓✗ |
1 |
q.w() = xyzw[3]; |
88 |
✓✗✓✗ ✓✗ |
1 |
q.vec() = xyzw.head<3>(); |
89 |
} |
||
90 |
|||
91 |
✓✗ | 2 |
if (std::fabs(1 - q.squaredNorm()) < 1e-4) { |
92 |
hppDout(warning, "Quaternion is not normalized."); |
||
93 |
} |
||
94 |
✓✗ | 2 |
q.normalize(); |
95 |
|||
96 |
✓✗ | 2 |
R = q.matrix(); |
97 |
} |
||
98 |
|||
99 |
✓✗ | 4 |
position_ = Transform3f(R, xyz); |
100 |
4 |
} |
|
101 |
} // namespace srdf |
||
102 |
} // namespace manipulation |
||
103 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |