GCC Code Coverage Report

Directory: ./
File: src/device.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 202 305 66.2%
Branches: 171 530 32.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Author: NMansard
4 //
5 //
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 #include <pinocchio/fwd.hpp>
32
33 // include pinocchio first
34
35 #include <coal/BV/AABB.h>
36 #include <coal/BVH/BVH_model.h>
37
38 #include <Eigen/Core>
39 #include <boost/foreach.hpp>
40 #include <boost/serialization/export.hpp>
41 #include <hpp/pinocchio/device.hh>
42 #include <hpp/pinocchio/fwd.hh>
43 #include <hpp/pinocchio/joint-collection.hh>
44 #include <hpp/util/serialization.hh>
45 #include <pinocchio/algorithm/geometry.hpp>
46 #include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
47 #include <pinocchio/algorithm/model.hpp>
48 #include <pinocchio/multibody/geometry.hpp>
49 #include <pinocchio/multibody/model.hpp>
50 #include <pinocchio/serialization/model.hpp>
51 // #include <hpp/pinocchio/distance-result.hh>
52 #include <hpp/pinocchio/body.hh>
53 #include <hpp/pinocchio/collision-object.hh>
54 #include <hpp/pinocchio/extra-config-space.hh>
55 #include <hpp/pinocchio/gripper.hh>
56 #include <hpp/pinocchio/joint.hh>
57 #include <hpp/pinocchio/liegroup-space.hh>
58 #include <hpp/pinocchio/liegroup.hh>
59 #include <hpp/pinocchio/serialization.hh>
60
61 namespace hpp {
62 namespace pinocchio {
63 const ::pinocchio::FrameType all_joint_type =
64 (::pinocchio::FrameType)(::pinocchio::JOINT | ::pinocchio::FIXED_JOINT);
65
66 30 Device::Device(const std::string& name)
67
4/8
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 30 times.
✗ Branch 17 not taken.
 30 : AbstractDevice(), name_(name), weakPtr_() {
68 30 invalidate();
69
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 createData();
70
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 createGeomData();
71
72
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 numberDeviceData(1);
73 30 }
74
75 ✗ Device::Device(const Device& other)
76 ✗ : AbstractDevice(other.model_, other.geomModel_),
77 ✗ d_(other.d_),
78 ✗ name_(other.name_),
79 ✗ grippers_(),
80 ✗ extraConfigSpace_(other.extraConfigSpace_),
81 ✗ weakPtr_(),
82 ✗ datas_() {
83 ✗ numberDeviceData(other.numberDeviceData());
84 }
85
86 118 Device::~Device() { datas_.clear(); }
87
88 155 void Device::numberDeviceData(const size_type& s) {
89 // Delete current device datas
90
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 155 datas_.clear();
91 // Create new device datas
92
1/2
✓ Branch 2 taken 155 times.
✗ Branch 3 not taken.
 155 std::vector<DeviceData*> datas((std::size_t)s);
93
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 155 times.
 255 for (std::size_t i = 0; i < (std::size_t)s; ++i)
94
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 datas[i] = new DeviceData(d_);
95
1/2
✓ Branch 3 taken 155 times.
✗ Branch 4 not taken.
 155 datas_.push_back(datas.begin(), datas.end());
96 155 }
97
98 124 size_type Device::numberDeviceData() const { return (size_type)datas_.size(); }
99
100 // static method
101 28 DevicePtr_t Device::create(const std::string& name) {
102
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
 28 DevicePtr_t res = DevicePtr_t(new Device(name)); // init shared ptr
103
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 res->init(res);
104 28 return res;
105 }
106
107 // static method
108 ✗ DevicePtr_t Device::createCopy(const DevicePtr_t& other) {
109 ✗ DevicePtr_t res = DevicePtr_t(new Device(*other)); // init shared ptr
110 ✗ res->initCopy(res, *other);
111 ✗ return res;
112 }
113
114 // static method
115 ✗ DevicePtr_t Device::createCopyConst(const DeviceConstPtr_t& device) {
116 ✗ DevicePtr_t res = Device::create(device->name()); // init shared ptr
117 /* The copy of Pinocchio::Model is not implemented yet. */
118 /* We need this feature to finish the implementation of this method. */
119 ✗ assert(false && "TODO: createCopyConst is not implemented yet.");
120 return res;
121 }
122
123 30 void Device::init(const DeviceWkPtr_t& weakPtr) {
124 30 weakPtr_ = weakPtr;
125 30 d_.devicePtr_ = weakPtr;
126 30 }
127
128 ✗ void Device::initCopy(const DeviceWkPtr_t& weakPtr, const Device& other) {
129 ✗ init(weakPtr);
130 ✗ grippers_.resize(other.grippers_.size());
131 ✗ for (std::size_t i = 0; i < grippers_.size(); ++i)
132 ✗ grippers_[i] = Gripper::createCopy(other.grippers_[i], weakPtr);
133 }
134
135 60 void Device::createData() {
136
1/2
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 d_.data_ = DataPtr_t(new Data(model()));
137 // We assume that model is now complete and state can be resized.
138 60 resizeState();
139 60 }
140
141 60 void Device::createGeomData() {
142
2/4
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 d().geomData_ = GeomDataPtr_t(new GeomData(geomModel()));
143 60 ::pinocchio::computeBodyRadius(model(), geomModel(), geomData());
144 60 d().invalidate();
145
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 numberDeviceData(numberDeviceData());
146 60 }
147
148 1 void Device::removeJoints(const std::vector<std::string>& jointNames,
149 Configuration_t referenceConfig) {
150
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::vector<JointIndex> joints(jointNames.size());
151
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::transform(jointNames.begin(), jointNames.end(), joints.begin(),
152 4 [this](const std::string& n) {
153 2 auto id = model_->getJointId(n);
154
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 2 if (id >= model_->joints.size())
155 ✗ throw std::invalid_argument("Device " + name_ +
156 " does not have "
157 ✗ "any joint named " +
158 ✗ n);
159 2 return id;
160 });
161
162
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 ModelPtr_t nModel(new Model);
163
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 GeomModelPtr_t nGeomModel(new GeomModel);
164
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ::pinocchio::buildReducedModel(*model_, *geomModel_, joints,
165
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 referenceConfig.head(model_->nq), *nModel,
166 1 *nGeomModel);
167 1 model_ = nModel;
168 1 geomModel_ = nGeomModel;
169
170 // update the grippers
171
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::transform(grippers_.begin(), grippers_.end(), grippers_.begin(),
172 ✗ [this](GripperPtr_t g) {
173 ✗ return Gripper::create(g->name(), this->weakPtr_);
174 });
175
176 1 invalidate();
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 createData();
178
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 createGeomData();
179
180
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 numberDeviceData(numberDeviceData());
181 1 }
182
183 /* ---------------------------------------------------------------------- */
184 /* --- JOINT ------------------------------------------------------------ */
185 /* ---------------------------------------------------------------------- */
186
187 18 JointPtr_t Device::rootJoint() const {
188
1/2
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 18 return Joint::create(weakPtr_.lock(), 1);
189 }
190
191 ✗ Frame Device::rootFrame() const {
192 ✗ return Frame(weakPtr_.lock(),
193 ✗ model().getFrameId("root_joint", all_joint_type));
194 }
195
196 81 size_type Device::nbJoints() const {
197 81 return size_type(model().joints.size() - 1);
198 }
199
200 80 JointPtr_t Device::jointAt(const size_type& i) const {
201
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 80 times.
 80 assert(i < nbJoints());
202
1/2
✓ Branch 3 taken 80 times.
✗ Branch 4 not taken.
 80 return Joint::create(weakPtr_.lock(), JointIndex(i + 1));
203 }
204
205 ✗ JointPtr_t Device::getJointAtConfigRank(const size_type& r) const {
206 // BOOST_FOREACH( const JointModel & j, // Skip "universe" joint
207 // std::make_pair(model_->joints.begin()+1,model_->joints.end()) )
208 ✗ BOOST_FOREACH (const JointModel& j, model().joints) {
209 ✗ if (j.id() == 0) continue; // Skip "universe" joint
210 ✗ const size_type iq = r - j.idx_q();
211 ✗ if (0 <= iq && iq < j.nq()) return Joint::create(weakPtr_.lock(), j.id());
212 }
213 ✗ assert(false && "The joint at config rank has not been found");
214 return JointPtr_t();
215 }
216
217 ✗ JointPtr_t Device::getJointAtVelocityRank(const size_type& r) const {
218 ✗ BOOST_FOREACH (const JointModel& j, model().joints) {
219 ✗ if (j.id() == 0) continue; // Skip "universe" joint
220 ✗ const size_type iv = r - j.idx_v();
221 ✗ if (0 <= iv && iv < j.nv()) return Joint::create(weakPtr_.lock(), j.id());
222 }
223 ✗ assert(false && "The joint at velocity rank has not been found");
224 return JointPtr_t();
225 }
226
227 30 JointPtr_t Device::getJointByName(const std::string& name) const {
228
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 28 times.
 30 if (!model().existJointName(name))
229
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 throw std::runtime_error("Device " + name_ +
230
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 8 " does not have any joint named " + name);
231 28 JointIndex id = model().getJointId(name);
232
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 return Joint::create(weakPtr_.lock(), id);
233 }
234
235 12 JointPtr_t Device::getJointByBodyName(const std::string& name) const {
236
3/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
✓ Branch 5 taken 2 times.
 12 if (model().existFrame(name)) {
237
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 FrameIndex bodyId = model().getFrameId(name);
238
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 if (model().frames[bodyId].type == ::pinocchio::BODY) {
239 10 JointIndex jointId = model().frames[bodyId].parent;
240 // assert(jointId>=0);
241
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 10 assert((std::size_t)jointId < model().joints.size());
242
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 return Joint::create(weakPtr_.lock(), jointId);
243 }
244 }
245
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 throw std::runtime_error("Device " + name_ +
246
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 8 " has no joint with body of name " + name);
247 }
248
249 8 Frame Device::getFrameByName(const std::string& name) const {
250
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
 8 if (!model().existFrame(name))
251 ✗ throw std::logic_error("Device " + name_ +
252 ✗ " does not have any frame named " + name);
253
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 FrameIndex id = model().getFrameId(name);
254
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 return Frame(weakPtr_.lock(), id);
255 }
256
257 168 size_type Device::configSize() const {
258 168 return model().nq + extraConfigSpace_.dimension();
259 }
260
261 171 size_type Device::numberDof() const {
262 171 return model().nv + extraConfigSpace_.dimension();
263 }
264
265 /* ---------------------------------------------------------------------- */
266 /* --- CONFIG ----------------------------------------------------------- */
267 /* ---------------------------------------------------------------------- */
268
269 62 void Device::resizeState() {
270 62 d_.invalidate();
271 62 d_.currentConfiguration_ = neutralConfiguration();
272
1/2
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
 62 d_.currentVelocity_ = vector_t::Zero(numberDof());
273
1/2
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
 62 d_.currentAcceleration_ = vector_t::Zero(numberDof());
274 62 d_.jointJacobians_.resize((std::size_t)model().njoints);
275
276 62 configSpace_ = LiegroupSpace::empty();
277 62 configSpaceRnxSOn_ = LiegroupSpace::empty();
278 62 const Model& m(model());
279
2/2
✓ Branch 1 taken 557 times.
✓ Branch 2 taken 62 times.
 619 for (JointIndex i = 1; i < m.joints.size(); ++i) {
280
3/6
✓ Branch 2 taken 557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 557 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 557 times.
✗ Branch 11 not taken.
 557 *configSpace_ *= Joint(weakPtr_, i).configurationSpace();
281
3/6
✓ Branch 2 taken 557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 557 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 557 times.
✗ Branch 11 not taken.
 557 *configSpaceRnxSOn_ *= Joint(weakPtr_, i).RnxSOnConfigurationSpace();
282 }
283
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 59 times.
 62 if (extraConfigSpace_.dimension() > 0) {
284 LiegroupSpacePtr_t extra =
285
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 LiegroupSpace::create(extraConfigSpace_.dimension());
286
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 *configSpace_ *= extra;
287
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 *configSpaceRnxSOn_ *= extra;
288 3 }
289
290
1/2
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
 62 numberDeviceData(numberDeviceData());
291 62 }
292
293 82 Configuration_t Device::neutralConfiguration() const {
294 82 const Model& m(model());
295
1/2
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
 82 Configuration_t n(configSize());
296
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 ::pinocchio::neutral(m, n.head(m.nq));
297
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 n.tail(extraConfigSpace_.dimension()).setZero();
298 82 return n;
299 }
300
301 13 void Device::addJointConstraint(JointLinearConstraint constraint) {
302
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 assert(constraint.joint && constraint.reference);
303
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 13 assert(constraint.joint->configSize() == 1);
304
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 13 assert(constraint.reference->configSize() == 1);
305
306 13 jointConstraints_.push_back(constraint);
307 13 }
308
309 ✗ std::ostream& Device::print(std::ostream& os) const {
310 ✗ os << model() << iendl;
311 // Print frame names
312 ✗ os << "Frames" << std::endl;
313 ✗ for (FrameIndex i = 0; i < (FrameIndex)model().nframes; ++i) {
314 ✗ const ::pinocchio::Frame& frame(model().frames[i]);
315 ✗ os << frame.name << "\t parent:" << model().names[frame.parent]
316 ✗ << std::endl;
317 }
318 ✗ if (jointConstraints_.size() > 0)
319 ✗ os << "Joint linear constraints:" << incindent;
320 ✗ for (std::size_t i = 0; i < jointConstraints_.size(); ++i)
321 ✗ os << iendl << jointConstraints_[i].joint->name() << " = "
322 ✗ << jointConstraints_[i].multiplier << " * "
323 ✗ << jointConstraints_[i].reference->name() << " + "
324 ✗ << jointConstraints_[i].offset;
325 ✗ return os << decindent << std::endl;
326 }
327
328 /* ---------------------------------------------------------------------- */
329 /* --- COLLISIONS ------------------------------------------------------- */
330 /* ---------------------------------------------------------------------- */
331
332 ✗ BodyPtr_t Device::obstacles() const {
333 ✗ return BodyPtr_t(new Body(weakPtr_.lock(), 0));
334 }
335
336 ✗ size_type Device::nbObjects() const {
337 ✗ return (size_type)geomModel().geometryObjects.size();
338 }
339
340 ✗ CollisionObjectPtr_t Device::objectAt(const size_type& i) const {
341 ✗ assert(i < nbObjects());
342 return CollisionObjectPtr_t(
343 ✗ new CollisionObject(weakPtr_.lock(), (GeomIndex)i));
344 }
345
346 ✗ bool Device::collisionTest(const bool stopAtFirstCollision) {
347 /* Following hpp::model API, the forward kinematics (joint placement) is
348 * supposed to have already been computed. */
349 ✗ updateGeometryPlacements();
350 ✗ return ::pinocchio::computeCollisions(geomModel(), geomData(),
351 ✗ stopAtFirstCollision);
352 }
353
354 ✗ void Device::computeDistances() {
355 /* Following hpp::model API, the forward kinematics (joint placement) is
356 * supposed to have already been computed. */
357 ✗ updateGeometryPlacements();
358 ✗ ::pinocchio::computeDistances(geomModel(), geomData());
359 }
360
361 ✗ const DistanceResults_t& Device::distanceResults() const {
362 ✗ return geomData().distanceResults;
363 }
364
365 /* ---------------------------------------------------------------------- */
366 /* --- Bounding box ----------------------------------------------------- */
367 /* ---------------------------------------------------------------------- */
368
369 struct AABBStep : public ::pinocchio::fusion::JointUnaryVisitorBase<AABBStep> {
370 typedef boost::fusion::vector<const Model&, Configuration_t, bool,
371 coal::AABB&>
372 ArgsType;
373
374 template <typename JointModel>
375 6 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
376 const Model& model, Configuration_t q, bool initializeAABB,
377 coal::AABB& aabb) {
378 typedef typename RnxSOnLieGroupMap::operation<JointModel>::type LG_t;
379 /*
380 if (LG_t::NT == 0) {
381 aabb.min_.setZero();
382 aabb.max_.setZero();
383 return;
384 }
385 */
386
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 typename JointModel::JointDataDerived data(jmodel.createData());
387 // Set configuration to lower bound.
388
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 jmodel.jointConfigSelector(q).template head<LG_t::NT>() =
389
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.jointConfigSelector(model.lowerPositionLimit)
390
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 .template head<LG_t::NT>();
391
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.calc(data, q);
392
2/5
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 vector3_t min = data.M_accessor().translation();
393 // Set configuration to upper bound.
394
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 jmodel.jointConfigSelector(q).template head<LG_t::NT>() =
395
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.jointConfigSelector(model.upperPositionLimit)
396
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 .template head<LG_t::NT>();
397
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.calc(data, q);
398
2/5
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 vector3_t max = data.M_accessor().translation();
399
400 // This should not be required as it should be done in
401 // AABB::operator+=(Vec3f) for(int i = 0; i < 3; ++i) { if (min[i] > max[i])
402 // std::swap(min[i], max[i]);
403 // }
404 // aabb.min_ = min;
405 // aabb.max_ = max;
406
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 6 if (initializeAABB)
407
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 aabb = coal::AABB(min);
408 else
409 ✗ aabb += min;
410
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 aabb += max;
411 }
412 };
413
414 3 coal::AABB Device::computeAABB() const {
415 // TODO check that user has called
416
417 3 const Model& m(model());
418
419 // Compute maximal distance to parent joint.
420
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 std::vector<value_type> maxDistToParent(m.joints.size(), 0);
421
2/2
✓ Branch 1 taken 81 times.
✓ Branch 2 taken 3 times.
 84 for (JointIndex i = 1; i < m.joints.size(); ++i) {
422
1/2
✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
 162 Joint joint(weakPtr_.lock(), i);
423
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 joint.computeMaximalDistanceToParent();
424
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 maxDistToParent[i] = joint.maximalDistanceToParent();
425 81 }
426
427 // Compute maximal distance to root joint.
428
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 std::vector<value_type> maxDistToRoot(m.joints.size(), 0);
429
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 std::vector<value_type> distances(m.joints.size(), 0);
430
431 3 std::vector<JointIndex> rootIdxs;
432 3 std::vector<value_type> maxRadius;
433
2/2
✓ Branch 1 taken 81 times.
✓ Branch 2 taken 3 times.
 84 for (JointIndex i = 1; i < m.joints.size(); ++i) {
434
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 78 times.
 81 if (m.parents[i] == 0) // Moving child of universe
435 {
436
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 rootIdxs.push_back(i);
437
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 maxRadius.push_back(0);
438 // This is the root of a subtree.
439 // maxDistToRoot[i] = 0; // Already zero by initialization
440 } else {
441 78 maxDistToRoot[i] = maxDistToRoot[m.parents[i]] + maxDistToParent[i];
442 }
443
444
1/2
✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
 162 Body body(weakPtr_.lock(), i);
445
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 distances[i] = body.radius() + maxDistToRoot[i];
446
447 81 maxRadius.back() = std::max(maxRadius.back(), distances[i]);
448 81 }
449
450 // Compute AABB
451
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Configuration_t q(neutralConfiguration());
452
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 coal::AABB aabb;
453
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
 6 for (std::size_t k = 0; k < rootIdxs.size(); ++k) {
454 3 JointIndex i = rootIdxs[k];
455 3 value_type radius = maxRadius[k];
456
457
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 coal::AABB aabb_subtree;
458
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 AABBStep::run(m.joints[i], AABBStep::ArgsType(m, q, true, aabb_subtree));
459
460 // Move AABB
461
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 coal::rotate(aabb_subtree, m.jointPlacements[i].rotation());
462
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 coal::translate(aabb_subtree, m.jointPlacements[i].translation());
463
464 // Add radius
465
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 aabb_subtree.min_.array() -= radius;
466
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 aabb_subtree.max_.array() += radius;
467
468 // Merge back into previous one.
469
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (k == 0)
470
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 aabb = aabb_subtree;
471 else
472 ✗ aabb += aabb_subtree;
473 }
474 6 return aabb;
475 3 }
476
477 ✗ void replaceGeometryByConvexHull(GeomModel& gmodel,
478 const std::vector<std::string>& gnames) {
479 ✗ for (std::size_t i = 0; i < gnames.size(); ++i) {
480 ✗ if (!gmodel.existGeometryName(gnames[i]))
481 ✗ throw std::invalid_argument("Geometry " + gnames[i] +
482 " does not "
483 ✗ "exist.");
484 ✗ GeomIndex gid = gmodel.getGeometryId(gnames[i]);
485 ✗ GeometryObject& go = gmodel.geometryObjects[gid];
486 ✗ if (go.geometry->getObjectType() == coal::OT_BVH) {
487 coal::BVHModelBase* bvh =
488 ✗ dynamic_cast<coal::BVHModelBase*>(go.geometry.get());
489 ✗ assert(bvh != NULL);
490 ✗ bvh->buildConvexHull(false, "Qx");
491 ✗ go.geometry = bvh->convex;
492 }
493 }
494 }
495
496 template <typename To, typename Ti, typename UnaryOp>
497 inline std::vector<To> serialize_to(const std::vector<Ti>& in, UnaryOp op) {
498 std::vector<To> out;
499 out.reserve(in.size());
500 std::transform(in.begin(), in.end(), out.begin(), op);
501 return out;
502 }
503
504 template <class Archive>
505 2 void Device::save(Archive& ar, const unsigned int version) const {
506 (void)version;
507
508 2 auto* har = hpp::serialization::cast(&ar);
509
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(name_);
510 // write it is not present in the HPP archive.
511 2 bool written =
512
3/6
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 (!har || har->template get<hpp::pinocchio::Device>(name_, false) != this);
513
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(written);
514
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (written) {
515 // AbstractDevice
516
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(model_);
517 // ar & BOOST_SERIALIZATION_NVP(geomModel_);
518
519 // Device
520 // - grippers_
521 2 std::vector<FrameIndex> grippers;
522 2 std::transform(
523 grippers_.begin(), grippers_.end(), grippers.begin(),
524 ✗ [](const GripperPtr_t& g) -> FrameIndex { return g->frameId(); });
525
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(grippers);
526
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(jointConstraints_);
527
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(weakPtr_);
528
529
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
530
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
531
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);
532
533
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 size_type nbDeviceData = numberDeviceData();
534
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(nbDeviceData);
535 }
536 }
537
538 template <class Archive>
539 2 void Device::load(Archive& ar, const unsigned int version) {
540 (void)version;
541
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(name_);
542 bool written;
543
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(written);
544
545 2 auto* har = hpp::serialization::cast(&ar);
546 2 bool dummyDevice =
547
3/6
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 har && har->template get<hpp::pinocchio::Device>(name_, false);
548
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (written) {
549 // AbstractDevice
550
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(model_);
551 // ar & BOOST_SERIALIZATION_NVP(geomModel_);
552
553 // Device
554 // - grippers_
555 2 std::vector<FrameIndex> grippers;
556
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(grippers);
557
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(jointConstraints_);
558
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(weakPtr_);
559
560
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
561
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
562
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);
563
564 size_type nbDeviceData;
565
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(nbDeviceData);
566
567
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 if (!dummyDevice) { // If this device will not be thrown away, initialize
568 // it cleanly.
569 ✗ grippers_.reserve(grippers.size());
570 ✗ std::transform(grippers.begin(), grippers.end(), grippers_.begin(),
571 ✗ [this](FrameIndex i) -> GripperPtr_t {
572 ✗ return Gripper::create(model_->frames[i].name, weakPtr_);
573 });
574 ✗ createData();
575 ✗ createGeomData();
576 ✗ numberDeviceData(nbDeviceData);
577 }
578
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 } else if (!dummyDevice)
579 ✗ throw std::logic_error(
580 "This archive does not contain a valid Device "
581 "of name " +
582 ✗ name_);
583 }
584
585 HPP_SERIALIZATION_SPLIT_IMPLEMENT(Device);
586 } // namespace pinocchio
587 } // namespace hpp
588
589 namespace boost {
590 namespace serialization {
591 template <class Archive>
592 ✗ void serialize(Archive& ar, hpp::pinocchio::Device::JointLinearConstraint& c,
593 const unsigned int version) {
594 (void)version;
595 ✗ ar& make_nvp("offset", c.offset);
596 ✗ ar& make_nvp("multiplier", c.multiplier);
597 ✗ ar& make_nvp("joint", c.joint);
598 ✗ ar& make_nvp("reference", c.reference);
599 }
600 } // namespace serialization
601 } // namespace boost
602