GCC Code Coverage Report

Directory: ./
File: src/frame.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 107 122 87.7%
Branches: 93 168 55.4%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
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/pinocchio/body.hh>
30 #include <hpp/pinocchio/device.hh>
31 #include <hpp/pinocchio/frame.hh>
32 #include <hpp/pinocchio/joint-collection.hh>
33 #include <hpp/pinocchio/joint.hh>
34 #include <pinocchio/algorithm/frames.hpp>
35 #include <pinocchio/algorithm/jacobian.hpp>
36 #include <pinocchio/multibody/geometry.hpp>
37 #include <pinocchio/multibody/joint/fwd.hpp>
38
39 namespace hpp {
40 namespace pinocchio {
41 namespace {
42 5 void moveFrame(Model& model, GeomModel& geomModel, const FrameIndex& pF,
43 const Transform3s& new_jMf) {
44 5 ::pinocchio::Frame& f = model.frames[pF];
45
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 const Transform3s old_fMj = f.placement.inverse();
46
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 5 times.
 105 for (GeomIndex i = 0; i < geomModel.geometryObjects.size(); ++i) {
47 100 ::pinocchio::GeometryObject& go = geomModel.geometryObjects[i];
48
5/8
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 100 if (go.parentFrame == pF) go.placement = new_jMf * old_fMj * go.placement;
49 }
50
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 f.placement = new_jMf;
51 5 }
52 } // namespace
53
54 10 Frame::Frame(DeviceWkPtr_t device, FrameIndex indexInFrameList)
55 10 : devicePtr_(device), frameIndex_(indexInFrameList) {
56
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 10 assert(devicePtr_.lock());
57
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
 10 assert(devicePtr_.lock()->modelPtr());
58
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
 10 assert(std::size_t(frameIndex_) < model().frames.size());
59 10 }
60
61 61 void Frame::selfAssert() const {
62
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 61 times.
 61 assert(devicePtr_.lock());
63
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 61 times.
 61 assert(devicePtr_.lock()->modelPtr());
64
2/4
✓ Branch 3 taken 61 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 61 times.
 61 assert(devicePtr_.lock()->dataPtr());
65
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 61 times.
 61 assert(devicePtr_.lock()->model().frames.size() > std::size_t(frameIndex_));
66 61 }
67
68 14 inline Model& Frame::model() {
69 14 selfAssert();
70 14 return devicePtr_.lock()->model();
71 }
72 26 inline const Model& Frame::model() const {
73 26 selfAssert();
74 26 return devicePtr_.lock()->model();
75 }
76 8 const ::pinocchio::Frame& Frame::pinocchio() const {
77 8 return model().frames[index()];
78 }
79 1 inline ::pinocchio::Frame& Frame::pinocchio() {
80 1 return model().frames[index()];
81 }
82
83
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 DeviceData& Frame::data() const { return devicePtr_.lock()->d(); }
84
85 2 Frame Frame::parentFrame() const {
86 2 FrameIndex idParent = model().frames[frameIndex_].previousFrame;
87
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return Frame(devicePtr_, idParent);
88 }
89
90 6 bool Frame::isFixed() const { return pinocchio().type != ::pinocchio::JOINT; }
91
92 1 JointPtr_t Frame::joint() const {
93
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 return Joint::create(devicePtr_, pinocchio().parent);
94 }
95
96 ✗ bool Frame::isRootFrame() const { return index() == 0; }
97
98 1 const std::string& Frame::name() const {
99 1 selfAssert();
100 1 return pinocchio().name;
101 }
102
103 5 Transform3s Frame::currentTransformation() const {
104 5 return currentTransformation(data());
105 }
106
107 5 Transform3s Frame::currentTransformation(const DeviceData& d) const {
108
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 selfAssert();
109
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 const ::pinocchio::Frame f = model().frames[frameIndex_];
110
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (f.type == ::pinocchio::JOINT)
111
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 return d.data_->oMi[f.parent];
112 else
113
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 return d.data_->oMi[f.parent] * f.placement;
114 5 }
115
116 ✗ JointJacobian_t Frame::jacobian(const DeviceData& d) const {
117 ✗ selfAssert();
118 ✗ assert(robot()->computationFlag() & JACOBIAN);
119 ✗ JointJacobian_t jacobian = JointJacobian_t::Zero(6, model().nv);
120 ✗ ::pinocchio::getFrameJacobian(model(), *d.data_, frameIndex_,
121 ::pinocchio::LOCAL, jacobian);
122 ✗ return jacobian;
123 }
124
125 3 void Frame::setChildList() {
126
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 selfAssert();
127
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 4 if (!children_.empty()) return;
128
3/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
 3 if (!isFixed()) return;
129
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const Model& m = model();
130
131
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 std::vector<bool> visited(m.frames.size(), false);
132
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 std::vector<bool> isChild(m.frames.size(), false);
133
134 2 FrameIndex k = frameIndex_;
135
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 2 times.
 9 while (k > 0) {
136
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 visited[k] = true;
137 7 k = m.frames[k].previousFrame;
138 }
139
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 visited[0] = true;
140
141
2/2
✓ Branch 1 taken 232 times.
✓ Branch 2 taken 2 times.
 234 for (FrameIndex i = m.frames.size() - 1; i > 0; --i) {
142
3/4
✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✓ Branch 5 taken 112 times.
 232 if (visited[i]) continue;
143
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 112 visited[i] = true;
144 112 k = m.frames[i].previousFrame;
145
2/2
✓ Branch 1 taken 167 times.
✓ Branch 2 taken 102 times.
 269 while (m.frames[k].type != ::pinocchio::JOINT) {
146
3/4
✓ Branch 0 taken 157 times.
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 157 times.
✗ Branch 3 not taken.
 167 if (k == frameIndex_ || k == 0) break;
147 // if (visited[k]) {
148 // std::vector<FrameIndex>::iterator _k =
149 // std::find(children_.begin(), children_.end(), k);
150 // if (_k != children_.end())
151 // children_.erase(_k);
152 // k = frameIndex_;
153 // break;
154 // }
155
1/2
✓ Branch 1 taken 157 times.
✗ Branch 2 not taken.
 157 visited[k] = true;
156 157 k = m.frames[k].previousFrame;
157 }
158
3/4
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 102 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 112 if (k == frameIndex_) children_.push_back(i);
159 }
160 2 }
161
162 ✗ const Transform3s& Frame::positionInParentJoint() const {
163 ✗ return pinocchio().placement;
164 }
165
166 11 Transform3s Frame::positionInParentFrame() const {
167
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 selfAssert();
168
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 const Model& m = model();
169
1/2
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
 11 const ::pinocchio::Frame f = m.frames[index()];
170
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 return m.frames[f.previousFrame].placement.inverse() *
171 11 ((f.type == ::pinocchio::FIXED_JOINT) ? f.placement
172
3/4
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 6 times.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 33 : m.jointPlacements[f.parent]);
173 11 }
174
175 1 void Frame::positionInParentFrame(const Transform3s& p) {
176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 selfAssert();
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 setChildList();
178
179 1 devicePtr_.lock()->invalidate();
180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Model& m = model();
181 1 GeomModel& geomModel = devicePtr_.lock()->geomModel();
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ::pinocchio::Frame& me = pinocchio();
183 1 bool isJoint = (me.type == ::pinocchio::JOINT);
184 ✗ Transform3s fMj = (isJoint ? m.jointPlacements[me.parent].inverse()
185
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 : me.placement.inverse());
186
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (isJoint)
187 ✗ m.jointPlacements[me.parent] = m.frames[me.previousFrame].placement * p;
188 else
189
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 me.placement = m.frames[me.previousFrame].placement * p;
190
191
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::vector<bool> visited(m.frames.size(), false);
192
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 for (std::size_t i = 0; i < children_.size(); ++i) {
193 5 FrameIndex k = children_[i];
194
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
 5 if (m.frames[k].type == ::pinocchio::JOINT) k = m.frames[k].previousFrame;
195
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
 10 while (k != frameIndex_) {
196
3/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 5 times.
 9 if (visited[k]) break;
197
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 visited[k] = true;
198
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 moveFrame(m, geomModel, k, me.placement * fMj * m.frames[k].placement);
199 5 k = m.frames[k].previousFrame;
200 }
201 }
202
203 // Update joint placements
204
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 for (std::size_t i = 0; i < children_.size(); ++i) {
205 5 FrameIndex k = children_[i];
206
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 const ::pinocchio::Frame f = m.frames[k];
207
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (f.type == ::pinocchio::JOINT) {
208 3 m.jointPlacements[f.parent] =
209
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 6 me.placement * fMj * m.jointPlacements[f.parent];
210 }
211 5 }
212 1 }
213
214 ✗ std::ostream& Frame::display(std::ostream& os) const {
215 ✗ os << "Frame " << frameIndex_ << (isFixed() ? " (Fixed)" : "") << " : "
216 ✗ << name() << '\n';
217 ✗ return os << std::endl;
218 }
219
220 } // namespace pinocchio
221 } // namespace hpp
222