GCC Code Coverage Report

Directory: ./
File: src/device-data.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 40 68 58.8%
Branches: 30 78 38.5%
Line Branch Exec Source
1 // Copyright (c) 2018, 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/device-data.hh>
30 #include <hpp/pinocchio/joint-collection.hh>
31 #include <pinocchio/algorithm/center-of-mass.hpp>
32 #include <pinocchio/algorithm/frames.hpp>
33 #include <pinocchio/algorithm/geometry.hpp>
34 #include <pinocchio/algorithm/kinematics.hpp>
35 #include <pinocchio/multibody/data.hpp>
36 #include <pinocchio/multibody/geometry.hpp>
37 #include <pinocchio/multibody/model.hpp>
38
39 namespace hpp {
40 namespace pinocchio {
41
42
4/8
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 31 times.
✗ Branch 14 not taken.
 31 DeviceData::DeviceData() { invalidate(); }
43
44 100 DeviceData::DeviceData(const DeviceData& other)
45
1/2
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
 100 : data_(new Data(*other.data_)),
46
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 100 geomData_(new GeomData(*other.geomData_)),
47
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 currentConfiguration_(other.currentConfiguration_),
48
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 currentVelocity_(other.currentVelocity_),
49
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 currentAcceleration_(other.currentAcceleration_),
50 100 dataUpToDate_(other.dataUpToDate_),
51 100 frameUpToDate_(other.frameUpToDate_),
52 100 geomUpToDate_(other.geomUpToDate_),
53 100 devicePtr_(),
54
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 modelConf_(other.modelConf_.size()),
55
1/2
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
 100 jointJacobians_(other.jointJacobians_.size()) {}
56
57 2004 void checkComputationFlag(int flag) {
58 // Turn off unused parameter compilation warning.
59 (void)flag;
60 // a IMPLIES b === (b || ~a)
61 // velocity IMPLIES position
62
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2004 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2004 assert((flag & JOINT_POSITION) || (!(flag & VELOCITY)));
63 // acceleration IMPLIES velocity
64
2/4
✓ Branch 0 taken 2006 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2006 times.
 2004 assert((flag & VELOCITY) || (!(flag & ACCELERATION)));
65 // com IMPLIES position
66
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2004 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2004 assert((flag & JOINT_POSITION) || (!(flag & COM)));
67 // jacobian IMPLIES position
68
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2004 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2004 assert((flag & JOINT_POSITION) || (!(flag & JACOBIAN)));
69 2004 }
70
71 constexpr int FK_ACC = JOINT_POSITION | VELOCITY | ACCELERATION;
72 constexpr int FK_VEL = JOINT_POSITION | VELOCITY;
73 constexpr int FK_POS = JOINT_POSITION;
74 // JCOM is different from JACOBIAN | COM
75 // as one could do
76 // computeForwardKinematics(JACOBIAN);
77 // computeForwardKinematics(COM);
78 // computeForwardKinematics(COM | JACOBIAN);
79 // In the last call, if JCOM = COM | JACOBIAN, the COM Jacobian would not be
80 // computed.
81 constexpr int JCOM = COM << 1 | COM;
82
83 2009 void DeviceData::computeForwardKinematics(const ModelPtr_t& mptr, int flag) {
84 2009 checkComputationFlag(flag);
85
86 2006 const Model& model = *mptr;
87 2012 const size_type nq = model.nq;
88 2012 const size_type nv = model.nv;
89
90 // TODO pinocchio does not allow to pass currentConfiguration_.head(nq) as
91 // a reference. This line avoids dynamic memory allocation
92
1/2
✓ Branch 2 taken 2001 times.
✗ Branch 3 not taken.
 2012 modelConf_ = currentConfiguration_.head(nq);
93
94
1/4
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 2001 times.
✗ Branch 3 not taken.
 2001 switch (flag & FK_ACC) {
95 ✗ case FK_ACC:
96 ✗ if (!(dataUpToDate_ & ACCELERATION)) {
97 ✗ ::pinocchio::forwardKinematics(model, *data_, modelConf_,
98 ✗ currentVelocity_.head(nv),
99 ✗ currentAcceleration_.head(nv));
100 ✗ dataUpToDate_ = dataUpToDate_ | FK_ACC;
101 }
102 ✗ break;
103 ✗ case FK_VEL:
104 ✗ if (!(dataUpToDate_ & VELOCITY)) {
105 ✗ ::pinocchio::forwardKinematics(model, *data_, modelConf_,
106 ✗ currentVelocity_.head(nv));
107 ✗ dataUpToDate_ = dataUpToDate_ | FK_VEL;
108 }
109 ✗ break;
110 2001 case FK_POS:
111
2/2
✓ Branch 0 taken 2000 times.
✓ Branch 1 taken 1 times.
 2001 if (!(dataUpToDate_ & JOINT_POSITION)) {
112 2000 ::pinocchio::forwardKinematics(model, *data_, modelConf_);
113 2001 dataUpToDate_ = dataUpToDate_ | FK_POS;
114 }
115 2002 break;
116 ✗ default:
117 ✗ throw std::logic_error("Wrong computation flag.");
118 }
119
120
1/2
✓ Branch 0 taken 2004 times.
✗ Branch 1 not taken.
 2002 if (flag & COM) {
121
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1994 times.
 2004 if (flag & JACOBIAN) {
122
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 10 if (!(dataUpToDate_ & JCOM)) {
123 ✗ ::pinocchio::jacobianCenterOfMass(model, *data_, modelConf_, true);
124 ✗ dataUpToDate_ = dataUpToDate_ | JCOM | COM;
125 }
126 } else {
127
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1994 times.
 1994 if (!(dataUpToDate_ & COM)) {
128 ✗ ::pinocchio::centerOfMass(model, *data_, ::pinocchio::POSITION, true);
129 ✗ dataUpToDate_ = dataUpToDate_ | COM;
130 }
131 }
132 }
133
134
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1992 times.
 2002 if (flag & JACOBIAN) {
135
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (!(dataUpToDate_ & JACOBIAN)) {
136 10 ::pinocchio::computeJointJacobians(model, *data_, modelConf_);
137 10 dataUpToDate_ = dataUpToDate_ | JACOBIAN;
138 }
139 }
140 2002 }
141
142 ✗ void DeviceData::computeFramesForwardKinematics(const ModelPtr_t& mptr) {
143 ✗ if (frameUpToDate_) return;
144 ✗ computeForwardKinematics(mptr, JOINT_POSITION);
145
146 ✗ ::pinocchio::updateFramePlacements(*mptr, *data_);
147
148 ✗ frameUpToDate_ = true;
149 }
150
151 ✗ void DeviceData::updateGeometryPlacements(const ModelPtr_t& m,
152 const GeomModelPtr_t& gm) {
153 ✗ if (!geomUpToDate_) {
154 ✗ ::pinocchio::updateGeometryPlacements(*m, *data_, *gm, *geomData_);
155 ✗ geomUpToDate_ = true;
156 }
157 }
158 } // namespace pinocchio
159 } // namespace hpp
160