| 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 | 9 | void PositionFactory::finishTags() { | |
| 40 |
2/2✓ Branch 2 taken 5 times.
✓ Branch 3 taken 4 times.
|
9 | if (values().size() != 0) |
| 41 | 5 | computeTransformFromText(); | |
| 42 | else | ||
| 43 | 4 | computeTransformFromAttributes(); | |
| 44 | 9 | } | |
| 45 | |||
| 46 | 5 | void PositionFactory::computeTransformFromText() { | |
| 47 | 5 | const std::vector<float>& v = values(); | |
| 48 |
1/2✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | Quaternion_t q(v[3], v[4], v[5], v[6]); // w, x, y, z |
| 49 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | if (std::fabs(1 - q.squaredNorm()) < 1e-4) { |
| 50 | hppDout(warning, "Quaternion is not normalized."); | ||
| 51 | } | ||
| 52 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | q.normalize(); |
| 53 |
4/8✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
|
5 | position_ = Transform3s(q.matrix(), vector3_t(v[0], v[1], v[2])); |
| 54 | 5 | } | |
| 55 | |||
| 56 | 4 | void PositionFactory::computeTransformFromAttributes() { | |
| 57 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | vector3_t xyz(0, 0, 0); |
| 58 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
20 | if (hasAttribute("xyz")) parser::readSequence(getAttribute("xyz"), xyz, 3); |
| 59 | |||
| 60 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | bool a_rpy = hasAttribute("rpy"); |
| 61 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | bool a_wxyz = hasAttribute("wxyz"); |
| 62 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | bool a_xyzw = hasAttribute("xyzw"); |
| 63 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
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 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | matrix3_t R(matrix3_t::Identity()); |
| 68 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | if (a_rpy) { |
| 69 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vector3_t rpy; |
| 70 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | parser::readSequence(getAttribute("rpy"), rpy, 3); |
| 71 | |||
| 72 | typedef Eigen::AngleAxis<value_type> AngleAxis; | ||
| 73 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | R = AngleAxis(rpy[2], vector3_t::UnitZ()) * |
| 74 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | AngleAxis(rpy[1], vector3_t::UnitY()) * |
| 75 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | AngleAxis(rpy[0], vector3_t::UnitX()); |
| 76 |
4/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
|
3 | } else if (a_wxyz || a_xyzw) { |
| 77 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Quaternion_t q; |
| 78 | |||
| 79 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | if (a_wxyz) { |
| 80 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vector_t wxyz(4); |
| 81 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | parser::readSequence(getAttribute("wxyz"), wxyz, 4); |
| 82 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | q.w() = wxyz[0]; |
| 83 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | q.vec() = wxyz.tail<3>(); |
| 84 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | } else if (a_xyzw) { |
| 85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | vector_t xyzw(4); |
| 86 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | parser::readSequence(getAttribute("xyzw"), xyzw, 4); |
| 87 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | q.w() = xyzw[3]; |
| 88 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | q.vec() = xyzw.head<3>(); |
| 89 | 1 | } | |
| 90 | |||
| 91 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (std::fabs(1 - q.squaredNorm()) < 1e-4) { |
| 92 | hppDout(warning, "Quaternion is not normalized."); | ||
| 93 | } | ||
| 94 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | q.normalize(); |
| 95 | |||
| 96 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | R = q.matrix(); |
| 97 | } | ||
| 98 | |||
| 99 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | position_ = Transform3s(R, xyz); |
| 100 | 4 | } | |
| 101 | } // namespace srdf | ||
| 102 | } // namespace manipulation | ||
| 103 | } // namespace hpp | ||
| 104 |