GCC Code Coverage Report

Directory: ./
File: src/srdf/factories/handle.cc
Date: 2025-06-05 11:04:44
Exec Total Coverage
Lines: 25 59 42.4%
Functions: 1 2 50.0%
Branches: 32 150 21.3%
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/handle.hh"
30
31 #include <hpp/manipulation/device.hh>
32 #include <hpp/manipulation/handle.hh>
33 #include <hpp/pinocchio/joint.hh>
34 #include <hpp/util/debug.hh>
35 #include <hpp/util/pointer.hh>
36 #include <pinocchio/multibody/model.hpp>
37
38 #include "hpp/manipulation/srdf/factories/position.hh"
39
40 #ifndef TIXML_SSCANF
41 #define TIXML_SSCANF sscanf
42 #endif
43
44 namespace hpp {
45 namespace manipulation {
46 namespace srdf {
47 2 void HandleFactory::finishTags() {
48
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 ObjectFactoryList factories = getChildrenOfType("position");
49
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (factories.empty()) {
50
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 6 factories = getChildrenOfType("local_position");
51 hppDout(warning, "Use tag position instead of local_position");
52 }
53
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (factories.size() != 1)
54 ✗ throw std::invalid_argument("handle tag " + name() +
55 ✗ " should have exactly one <position>");
56
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 PositionFactory* pf = factories.front()->as<PositionFactory>();
57
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 localPosition_ = pf->position();
58
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 factories = getChildrenOfType("link");
59
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (factories.size() != 1)
60 ✗ throw std::invalid_argument("handle tag " + name() +
61 ✗ " should have exactly one <link>");
62
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 linkName_ = root()->prependPrefix(factories.front()->name());
63
64 /// Get the clearance
65 2 value_type clearance = 0;
66
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
 6 if (hasAttribute("clearance")) {
67 ✗ if (TIXML_SSCANF(getAttribute("clearance").c_str(), "%lf", &clearance) !=
68 1) {
69 hppDout(error, "Could not cast attribute clearance of tag "
70 << name() << " to double");
71 }
72 } else {
73 hppDout(warning,
74 "Missing attribute clearance of tag " << name() << ". Assuming 0");
75 }
76
77 /// Get the mask
78
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 6 factories = getChildrenOfType("mask");
79
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 std::vector<bool> mask(6, true);
80 2 if (factories.size() > 1) {
81 hppDout(warning,
82 "handle should have at most one <mask>. Using the first one");
83 }
84
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!factories.empty()) {
85 parser::SequenceFactory<bool>* mf =
86 ✗ factories.front()->as<parser::SequenceFactory<bool> >();
87 ✗ mask = mf->values();
88 }
89
90 /// Get the mask complement
91
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 6 factories = getChildrenOfType("mask_complement");
92
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 std::vector<bool> maskComp(6, false);
93 2 if (factories.size() > 1) {
94 hppDout(warning,
95 "handle should have at most one <mask_complement>. Using the first "
96 "one");
97 }
98 2 bool maskCompSpecified(false);
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!factories.empty()) {
100 ✗ maskCompSpecified = true;
101 parser::SequenceFactory<bool>* mf =
102 ✗ factories.front()->as<parser::SequenceFactory<bool> >();
103 ✗ maskComp = mf->values();
104 }
105
106 /// We have now all the information to build the handle.
107
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 DevicePtr_t d = HPP_DYNAMIC_PTR_CAST(Device, root()->device());
108
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (!d) {
109 hppDout(error, "Failed to create handle");
110 2 return;
111 }
112 ✗ const pinocchio::Model& model = d->model();
113 ✗ if (!model.existBodyName(linkName_))
114 ✗ throw std::invalid_argument("Link " + linkName_ +
115 ✗ " not found. Cannot create handle");
116 const ::pinocchio::Frame& linkFrame =
117 ✗ model.frames[model.getFrameId(linkName_)];
118 ✗ assert(linkFrame.type == ::pinocchio::BODY);
119 ✗ pinocchio::JointIndex index(0);
120 ✗ std::string jointName("universe");
121 ✗ JointPtr_t joint(Joint::create(d, linkFrame.parent));
122 ✗ if (joint) {
123 ✗ index = joint->index();
124 ✗ jointName = joint->name();
125 }
126 // Handle position is expressed in link frame. We need to express it in
127 // joint frame.
128 ✗ handle_ = Handle::create(root()->prependPrefix(name()),
129 ✗ linkFrame.placement * localPosition_, d, joint);
130 ✗ handle_->clearance(clearance);
131 ✗ handle_->mask(mask);
132 ✗ if (maskCompSpecified) handle_->maskComp(maskComp);
133 ✗ d->handles.add(handle_->name(), handle_);
134 ✗ assert(d->model().existFrame(jointName));
135 ✗ ::pinocchio::FrameIndex previousFrame(d->model().getFrameId(jointName));
136 ✗ d->model().addFrame(::pinocchio::Frame(handle_->name(), index, previousFrame,
137 ✗ linkFrame.placement * localPosition_,
138 ::pinocchio::OP_FRAME));
139 ✗ d->createData();
140
4/8
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 2 times.
 8 }
141
142 ✗ HandlePtr_t HandleFactory::handle() const { return handle_; }
143 } // namespace srdf
144 } // namespace manipulation
145 } // namespace hpp
146