GCC Code Coverage Report

Directory: ./
File: src/srdf/factories/gripper.cc
Date: 2025-06-05 11:04:44
Exec Total Coverage
Lines: 30 45 66.7%
Functions: 1 2 50.0%
Branches: 42 124 33.9%
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/gripper.hh"
30
31 #include <hpp/manipulation/device.hh>
32 #include <hpp/pinocchio/gripper.hh>
33 #include <hpp/util/debug.hh>
34 #include <hpp/util/pointer.hh>
35 #include <pinocchio/multibody/model.hpp>
36
37 #include "hpp/manipulation/srdf/factories/position.hh"
38
39 #ifndef TIXML_SSCANF
40 #define TIXML_SSCANF sscanf
41 #endif
42
43 namespace hpp {
44 namespace manipulation {
45 namespace srdf {
46 2 void GripperFactory::finishTags() {
47
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");
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (factories.empty()) {
49 ✗ factories = getChildrenOfType("handle_position_in_joint");
50 hppDout(warning, "Use tag position instead of handle_position_in_joint");
51 }
52
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (factories.size() != 1)
53 ✗ throw std::invalid_argument("gripper tag " + name() +
54 ✗ " should have exactly one <position>");
55
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 PositionFactory* pf = factories.front()->as<PositionFactory>();
56
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 localPosition_ = pf->position();
57
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("gripper 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
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 const std::string& gripperName = root()->prependPrefix(name());
64
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (gripperName == linkName_)
65 ✗ throw std::invalid_argument("Gripper " + gripperName +
66 ✗ " cannot have the same name as link " +
67 ✗ linkName_ +
68 ". "
69 ✗ "Cannot create gripper");
70
71 /// Get the clearance
72 2 value_type clearance = 0;
73
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 6 if (hasAttribute("clearance")) {
74
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 6 if (TIXML_SSCANF(getAttribute("clearance").c_str(), "%lf", &clearance) !=
75 1) {
76 hppDout(error, "Could not cast attribute clearance of tag "
77 << name() << " to double");
78 }
79 } else {
80 hppDout(warning,
81 "Missing attribute clearance of tag " << name() << ". Assuming 0");
82 }
83
84 /// We have now all the information to build the handle.
85
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 DevicePtr_t d = root()->device();
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (!d) {
87 hppDout(error, "Failed to create gripper");
88 ✗ return;
89 }
90 2 const pinocchio::Model& model = d->model();
91
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 if (!model.existBodyName(linkName_))
92 ✗ throw std::invalid_argument("Link " + linkName_ +
93 ✗ " not found. Cannot create gripper");
94
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 pinocchio::FrameIndex linkFrameId = model.getFrameId(linkName_);
95 2 const ::pinocchio::Frame& linkFrame = model.frames[linkFrameId];
96
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(linkFrame.type == ::pinocchio::BODY);
97 // Gripper position is expressed in link frame. We need to compute
98 // the position in joint frame.
99
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 if (model.existFrame(gripperName, ::pinocchio::OP_FRAME))
100 ✗ throw std::runtime_error("Could not add gripper frame of gripper " +
101 ✗ gripperName);
102
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 4 d->model().addFrame(::pinocchio::Frame(
103
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 gripperName, linkFrame.parent, linkFrameId,
104
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 linkFrame.placement * localPosition_, ::pinocchio::OP_FRAME));
105
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 d->createData();
106
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 gripper_ = pinocchio::Gripper::create(gripperName, root()->device());
107 2 gripper_->clearance(clearance);
108
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 d->grippers.add(gripper_->name(), gripper_);
109 hppDout(info, "Add gripper " << gripper_->name() << "\n\tattached to joint "
110 << model.names[model.frames[linkFrameId].parent]
111 << " with position "
112 << gripper_->objectPositionInJoint()
113 << "\n\tclearance " << clearance);
114
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 }
115
116 ✗ GripperPtr_t GripperFactory::gripper() const { return gripper_; }
117 } // namespace srdf
118 } // namespace manipulation
119 } // namespace hpp
120