device.cc 21.6 KB
Newer Older
Nicolas Mansard's avatar
Nicolas Mansard committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//
// Copyright (c) 2016 CNRS
// Author: NMansard
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

20
#include <hpp/pinocchio/device.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
21
22

#include <boost/foreach.hpp>
23
24
#include <boost/serialization/export.hpp>

Joseph Mirabel's avatar
Joseph Mirabel committed
25
26
#include <Eigen/Core>

27
28
#include <hpp/util/serialization.hh>

Joseph Mirabel's avatar
Joseph Mirabel committed
29
#include <hpp/fcl/BV/AABB.h>
30
#include <hpp/fcl/BVH/BVH_model.h>
Joseph Mirabel's avatar
Joseph Mirabel committed
31

Nicolas Mansard's avatar
Nicolas Mansard committed
32
#include <pinocchio/algorithm/geometry.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
33
#include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
34
35
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/multibody/model.hpp>
36
#include <pinocchio/serialization/model.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
37
38

#include <hpp/pinocchio/fwd.hh>
39
#include <hpp/pinocchio/joint-collection.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
40
//#include <hpp/pinocchio/distance-result.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
41
#include <hpp/pinocchio/body.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
42
#include <hpp/pinocchio/extra-config-space.hh>
43
#include <hpp/pinocchio/collision-object.hh>
44
#include <hpp/pinocchio/gripper.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
45
#include <hpp/pinocchio/joint.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
46
#include <hpp/pinocchio/liegroup.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
47
#include <hpp/pinocchio/liegroup-space.hh>
48
49
#include <hpp/pinocchio/serialization.hh>

50
51
namespace hpp {
  namespace pinocchio {
52
53
    const ::pinocchio::FrameType all_joint_type =
      (::pinocchio::FrameType) (::pinocchio::JOINT | ::pinocchio::FIXED_JOINT);
54
55
56

    Device::
    Device(const std::string& name)
57
      : AbstractDevice ()
58
59
      , name_ (name)
      , weakPtr_()
60
61
    {
      invalidate();
62
63
      createData();
      createGeomData();
64
65

      numberDeviceData(1);
66
    }
67

68
    Device::Device(const Device& other)
69
      : AbstractDevice (other.model_, other.geomModel_)
70
      , d_ (other.d_)
71
72
73
74
      , name_ (other.name_)
      , grippers_ ()
      , extraConfigSpace_ (other.extraConfigSpace_)
      , weakPtr_()
75
76
      , datas_ ()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
77
      numberDeviceData(other.numberDeviceData());
78
    }
79

80
81
    Device::~Device ()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
82
      datas_.clear();
83
84
85
86
    }

    void Device::numberDeviceData (const size_type& s)
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
      // Delete current device datas
      datas_.clear();
      // Create new device datas
Joseph Mirabel's avatar
Joseph Mirabel committed
90
91
      std::vector<DeviceData*> datas ((std::size_t)s);
      for (std::size_t i = 0; i < (std::size_t)s; ++i) datas[i] = new DeviceData (d_);
Joseph Mirabel's avatar
Joseph Mirabel committed
92
      datas_.push_back (datas.begin(), datas.end());
93
94
    }

Joseph Mirabel's avatar
Add doc    
Joseph Mirabel committed
95
96
97
98
99
    size_type Device::numberDeviceData () const
    {
      return (size_type)datas_.size();
    }

100
101
102
103
104
    // static method
    DevicePtr_t Device::
    create (const std::string & name)
    {
      DevicePtr_t res = DevicePtr_t(new Device(name)); // init shared ptr
Joseph Mirabel's avatar
Joseph Mirabel committed
105
      res->init (res);
106
107
108
109
110
      return res;
    }
    
    // static method
    DevicePtr_t Device::
111
    createCopy (const DevicePtr_t& other)
112
    {
113
114
      DevicePtr_t res = DevicePtr_t(new Device(*other)); // init shared ptr
      res->initCopy (res, *other);
115
116
117
118
119
120
121
122
123
124
125
126
127
128
      return res;
    }

    // static method
    DevicePtr_t Device::
    createCopyConst (const DeviceConstPtr_t& device)
    {
      DevicePtr_t res = Device::create(device->name()); // init shared ptr
      /* The copy of Pinocchio::Model is not implemented yet. */
      /* We need this feature to finish the implementation of this method. */
      assert( false && "TODO: createCopyConst is not implemented yet." );
      return res;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
129
130
131
    void Device::init(const DeviceWkPtr_t& weakPtr)
    {
      weakPtr_ = weakPtr;
132
      d_.devicePtr_ = weakPtr;
Joseph Mirabel's avatar
Joseph Mirabel committed
133
134
    }

135
136
137
138
139
140
141
142
    void Device::initCopy(const DeviceWkPtr_t& weakPtr, const Device& other)
    {
      init(weakPtr);
      grippers_.resize (other.grippers_.size());
      for (std::size_t i = 0; i < grippers_.size(); ++i)
        grippers_[i] = Gripper::createCopy(other.grippers_[i], weakPtr);
    }

143
144
145
    void Device::
    createData()
    {
146
      d_.data_ = DataPtr_t( new Data(model()) );
147
148
149
150
151
152
153
      // We assume that model is now complete and state can be resized.
      resizeState(); 
    }

    void Device::
    createGeomData()
    {
154
      d().geomData_ = GeomDataPtr_t( new GeomData(geomModel()) );
Joseph Mirabel's avatar
Joseph Mirabel committed
155
      ::pinocchio::computeBodyRadius(model(),geomModel(),geomData());
156
      d().invalidate();
Joseph Mirabel's avatar
Joseph Mirabel committed
157
      numberDeviceData(numberDeviceData());
158
    }
159
160
161
162
163
164
165
166

    void Device::controlComputation (const Computation_t& flag)
    {
      AbstractDevice::controlComputation (flag);
      // TODO this should not be done in controlComputation
      // It should be done in another function (like controlComputations)
      // as it might be a desired behaviour to have different computation options
      // in different DeviceData.
Joseph Mirabel's avatar
Joseph Mirabel committed
167
      numberDeviceData(numberDeviceData());
168
    }
169
    
Nicolas Mansard's avatar
Nicolas Mansard committed
170
171
172
173
    /* ---------------------------------------------------------------------- */
    /* --- JOINT ------------------------------------------------------------ */
    /* ---------------------------------------------------------------------- */

Joseph Mirabel's avatar
Joseph Mirabel committed
174
    JointPtr_t Device::rootJoint () const
175
    {
176
      return Joint::create(weakPtr_.lock(),1);
177
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
178

Joseph Mirabel's avatar
Joseph Mirabel committed
179
180
    Frame Device::rootFrame () const
    {
181
      return Frame(weakPtr_.lock(), model().getFrameId("root_joint", all_joint_type));
Joseph Mirabel's avatar
Joseph Mirabel committed
182
183
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
184
185
186
187
188
189
190
191
    size_type Device::nbJoints () const
    {
      return size_type(model().joints.size() - 1);
    }

    JointPtr_t Device::jointAt (const size_type& i) const
    {
      assert (i < nbJoints());
Joseph Mirabel's avatar
Joseph Mirabel committed
192
      return Joint::create(weakPtr_.lock(),JointIndex(i+1));
Joseph Mirabel's avatar
Joseph Mirabel committed
193
194
    }

195
196
197
    JointPtr_t Device::
    getJointAtConfigRank (const size_type& r) const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
198
      //BOOST_FOREACH( const JointModel & j, // Skip "universe" joint
199
      //std::make_pair(model_->joints.begin()+1,model_->joints.end()) )
Joseph Mirabel's avatar
Joseph Mirabel committed
200
      BOOST_FOREACH( const JointModel & j, model().joints )
Nicolas Mansard's avatar
Nicolas Mansard committed
201
        {
202
          if( j.id()==0 ) continue; // Skip "universe" joint
203
          const size_type iq = r - j.idx_q();
204
          if( 0 <= iq && iq < j.nq() ) return Joint::create(weakPtr_.lock(),j.id());
Nicolas Mansard's avatar
Nicolas Mansard committed
205
206
207
        }
      assert(false && "The joint at config rank has not been found");
      return JointPtr_t();
208
209
210
211
    }

    JointPtr_t Device::
    getJointAtVelocityRank (const size_type& r) const
Nicolas Mansard's avatar
Nicolas Mansard committed
212
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
213
      BOOST_FOREACH( const JointModel & j,model().joints )
Nicolas Mansard's avatar
Nicolas Mansard committed
214
        {
215
          if( j.id()==0 ) continue; // Skip "universe" joint
216
          const size_type iv = r - j.idx_v();
217
          if( 0 <= iv && iv < j.nv() ) return Joint::create(weakPtr_.lock(),j.id());
Nicolas Mansard's avatar
Nicolas Mansard committed
218
219
220
221
        }
      assert(false && "The joint at velocity rank has not been found");
      return JointPtr_t();
    }
222
223
224

    JointPtr_t Device::
    getJointByName (const std::string& name) const
Nicolas Mansard's avatar
Nicolas Mansard committed
225
    {
226
      if(! model().existJointName(name))
227
228
229
	throw std::runtime_error ("Device " + name_ +
				  " does not have any joint named "
				  + name);
230
      JointIndex id = model().getJointId(name);
231
      return Joint::create(weakPtr_.lock(),id);
Nicolas Mansard's avatar
Nicolas Mansard committed
232
    }
233
234
235

    JointPtr_t Device::
    getJointByBodyName (const std::string& name) const
Nicolas Mansard's avatar
Nicolas Mansard committed
236
    {
237
      if (model().existFrame(name)) {
Joseph Mirabel's avatar
Joseph Mirabel committed
238
239
        FrameIndex bodyId = model().getFrameId(name);
        if (model().frames[bodyId].type == ::pinocchio::BODY) {
240
          JointIndex jointId = model().frames[bodyId].parent;
241
          //assert(jointId>=0);
242
          assert((std::size_t)jointId<model().joints.size());
243
          return Joint::create(weakPtr_.lock(),jointId);
244
245
246
247
248
        }
      }
      throw std::runtime_error ("Device " + name_ +
                                " has no joint with body of name "
                                + name);
Nicolas Mansard's avatar
Nicolas Mansard committed
249
    }
250

Joseph Mirabel's avatar
Joseph Mirabel committed
251
252
253
    Frame Device::
    getFrameByName (const std::string& name) const
    {
254
      if(! model().existFrame(name))
Joseph Mirabel's avatar
Joseph Mirabel committed
255
256
257
	throw std::logic_error ("Device " + name_ +
				" does not have any frame named "
				+ name);
258
      FrameIndex id = model().getFrameId(name);
Joseph Mirabel's avatar
Joseph Mirabel committed
259
260
261
      return Frame(weakPtr_.lock(), id);
    }

262
263
    size_type Device::
    configSize () const
Nicolas Mansard's avatar
Nicolas Mansard committed
264
    {
265
      return model().nq + extraConfigSpace_.dimension();
Nicolas Mansard's avatar
Nicolas Mansard committed
266
    }
267
268
269

    size_type Device::
    numberDof () const
Nicolas Mansard's avatar
Nicolas Mansard committed
270
    {
271
      return model().nv + extraConfigSpace_.dimension();
Nicolas Mansard's avatar
Nicolas Mansard committed
272
    }
273

Nicolas Mansard's avatar
Nicolas Mansard committed
274
    /* ---------------------------------------------------------------------- */
275
    /* --- CONFIG ----------------------------------------------------------- */
Nicolas Mansard's avatar
Nicolas Mansard committed
276
    /* ---------------------------------------------------------------------- */
277

Nicolas Mansard's avatar
Nicolas Mansard committed
278
279
280
    void Device::
    resizeState()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
281
      d_.invalidate();
282
283
284
      d_.currentConfiguration_ = neutralConfiguration();
      d_.currentVelocity_      = vector_t::Zero(numberDof());
      d_.currentAcceleration_  = vector_t::Zero(numberDof());
Joseph Mirabel's avatar
Joseph Mirabel committed
285
      d_.jointJacobians_.resize ((std::size_t)model().njoints);
Joseph Mirabel's avatar
Joseph Mirabel committed
286

287
      configSpace_ = LiegroupSpace::empty();
288
      configSpaceRnxSOn_ = LiegroupSpace::empty();
Joseph Mirabel's avatar
Joseph Mirabel committed
289
      const Model& m (model());
290
291
292
293
294
295
296
297
298
      for (JointIndex i = 1; i < m.joints.size(); ++i) {
        *configSpace_       *= Joint(weakPtr_, i).configurationSpace();
        *configSpaceRnxSOn_ *= Joint(weakPtr_, i).RnxSOnConfigurationSpace();
      }
      if (extraConfigSpace_.dimension() > 0) {
        LiegroupSpacePtr_t extra = LiegroupSpace::create (extraConfigSpace_.dimension());
        *configSpace_       *= extra;
        *configSpaceRnxSOn_ *= extra;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
299

Joseph Mirabel's avatar
Joseph Mirabel committed
300
      numberDeviceData(numberDeviceData());
Nicolas Mansard's avatar
Nicolas Mansard committed
301
    }
302

303
    Configuration_t Device::
Nicolas Mansard's avatar
Nicolas Mansard committed
304
    neutralConfiguration () const
305
    {
306
      const Model& m (model());
Joseph Mirabel's avatar
Joseph Mirabel committed
307
      Configuration_t n (configSize());
308
      ::pinocchio::neutral (m, n.head(m.nq));
309
310
311
      n.tail(extraConfigSpace_.dimension()).setZero();
      return n;
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
312

313
314
315
316
317
318
319
320
321
322
    void Device::
    addJointConstraint (JointLinearConstraint constraint)
    {
      assert (constraint.joint && constraint.reference);
      assert (constraint.joint    ->configSize() == 1);
      assert (constraint.reference->configSize() == 1);

      jointConstraints_.push_back (constraint);
    }

Nicolas Mansard's avatar
Nicolas Mansard committed
323
324
325
    std::ostream& Device::
    print (std::ostream& os) const
    {
326
327
328
329
330
331
332
333
334
335
      os << model() << iendl;
      if (jointConstraints_.size()>0)
        os << "Joint linear constraints:" << incindent;
      for (std::size_t i = 0; i < jointConstraints_.size(); ++i)
        os << iendl
          << jointConstraints_[i].joint->name() << " = "
          << jointConstraints_[i].multiplier << " * "
          << jointConstraints_[i].reference->name() << " + "
          << jointConstraints_[i].offset;
      return os << decindent << std::endl;
Nicolas Mansard's avatar
Nicolas Mansard committed
336
    }
337

338
339
340
341
    /* ---------------------------------------------------------------------- */
    /* --- COLLISIONS ------------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

Joseph Mirabel's avatar
Joseph Mirabel committed
342
343
344
345
346
    BodyPtr_t Device::obstacles () const
    {
      return BodyPtr_t( new Body(weakPtr_.lock(),0) );
    }

347
348
349
350
351
352
353
354
    size_type Device::nbObjects() const
    {
      return (size_type)geomModel().geometryObjects.size();
    }

    CollisionObjectPtr_t Device::objectAt (const size_type& i) const
    {
      assert (i < nbObjects());
Joseph Mirabel's avatar
Joseph Mirabel committed
355
      return CollisionObjectPtr_t (new CollisionObject(weakPtr_.lock(),(GeomIndex)i));
356
357
    }

358
    bool Device::collisionTest (const bool stopAtFirstCollision)
Nicolas Mansard's avatar
Nicolas Mansard committed
359
    {
360
361
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
362
      updateGeometryPlacements();
Joseph Mirabel's avatar
Joseph Mirabel committed
363
      return ::pinocchio::computeCollisions(geomModel(), geomData(),stopAtFirstCollision);
364
365
366
367
368
369
    }

    void Device::computeDistances ()
    {
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
370
      updateGeometryPlacements();
Joseph Mirabel's avatar
Joseph Mirabel committed
371
      ::pinocchio::computeDistances (geomModel(), geomData());
372
373
374
375
    }

    const DistanceResults_t& Device::distanceResults () const
    {
376
      return geomData().distanceResults;
Nicolas Mansard's avatar
Nicolas Mansard committed
377
    }
378

Joseph Mirabel's avatar
Joseph Mirabel committed
379
380
381
382
    /* ---------------------------------------------------------------------- */
    /* --- Bounding box ----------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

383
    struct AABBStep : public ::pinocchio::fusion::JointUnaryVisitorBase<AABBStep>
Joseph Mirabel's avatar
Joseph Mirabel committed
384
385
386
387
388
389
390
    {
      typedef boost::fusion::vector<const Model&,
                                    Configuration_t,
                                    bool,
                                    fcl::AABB&> ArgsType;

      template<typename JointModel>
Joseph Mirabel's avatar
Joseph Mirabel committed
391
      static void algo(const ::pinocchio::JointModelBase<JointModel> & jmodel,
Joseph Mirabel's avatar
Joseph Mirabel committed
392
393
394
395
396
                       const Model& model,
                       Configuration_t q,
                       bool initializeAABB,
                       fcl::AABB& aabb)
      {
Joseph Mirabel's avatar
Joseph Mirabel committed
397
        typedef typename RnxSOnLieGroupMap::operation<JointModel>::type LG_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
        /*
        if (LG_t::NT == 0) {
          aabb.min_.setZero();
          aabb.max_.setZero();
          return;
        }
        */
        typename JointModel::JointDataDerived data (jmodel.createData());
        // Set configuration to lower bound.
        jmodel.jointConfigSelector(q).template head<LG_t::NT>() =
          jmodel.jointConfigSelector(model.lowerPositionLimit).template head<LG_t::NT>();
        jmodel.calc(data, q);
        vector3_t min = data.M.translation();
        // Set configuration to upper bound.
        jmodel.jointConfigSelector(q).template head<LG_t::NT>() =
          jmodel.jointConfigSelector(model.upperPositionLimit).template head<LG_t::NT>();
        jmodel.calc(data, q);
        vector3_t max = data.M.translation();

        // This should not be required as it should be done in AABB::operator+=(Vec3f)
        // for(int i = 0; i < 3; ++i) {
          // if (min[i] > max[i]) std::swap(min[i], max[i]);
        // }
        // aabb.min_ = min;
        // aabb.max_ = max;
        if (initializeAABB) aabb = fcl::AABB(min);
        else                aabb += min;
        aabb += max;
      }

    };

    template <>
Joseph Mirabel's avatar
Joseph Mirabel committed
431
432
    void AABBStep::algo< JointModelComposite >(
        const ::pinocchio::JointModelBase< JointModelComposite > & jmodel,
Joseph Mirabel's avatar
Joseph Mirabel committed
433
434
435
436
437
438
439
440
        const Model& model,
        Configuration_t q,
        bool initializeAABB,
        fcl::AABB& aabb)
    {
      // TODO this should for but I did not test it.
      hppDout(warning, "Computing AABB of JointModelComposite should work but has never been tested");
      if (initializeAABB)  {
Joseph Mirabel's avatar
Joseph Mirabel committed
441
        JointModelComposite::JointDataDerived data = jmodel.createData();
Joseph Mirabel's avatar
Joseph Mirabel committed
442
443
444
        jmodel.calc(data, q);
        aabb = fcl::AABB(data.M.translation());
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
445
      ::pinocchio::details::Dispatch<AABBStep>::run(jmodel.derived(), AABBStep::ArgsType(model, q, false, aabb));
Joseph Mirabel's avatar
Joseph Mirabel committed
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
    }

    fcl::AABB Device::computeAABB() const
    {
      // TODO check that user has called

      const Model& m (model());

      // Compute maximal distance to parent joint.
      std::vector<value_type> maxDistToParent (m.joints.size(), 0);
      for (JointIndex i = 1; i < m.joints.size(); ++i)
      {
        Joint joint (weakPtr_.lock(), i);
        joint.computeMaximalDistanceToParent();
        maxDistToParent[i] = joint.maximalDistanceToParent();
      }

      // Compute maximal distance to root joint.
      std::vector<value_type> maxDistToRoot (m.joints.size(), 0);
      std::vector<value_type> distances (m.joints.size(), 0);

      std::vector<JointIndex> rootIdxs;
      std::vector<value_type> maxRadius;
      for (JointIndex i = 1; i < m.joints.size(); ++i)
      {
        if (m.parents[i] == 0) // Moving child of universe
        {
          rootIdxs.push_back(i);
          maxRadius.push_back(0);
          // This is the root of a subtree.
          // maxDistToRoot[i] = 0; // Already zero by initialization
        } else {
          maxDistToRoot[i] = maxDistToRoot[m.parents[i]] + maxDistToParent[i];
        }

        Body body (weakPtr_.lock(), i);
        distances[i] = body.radius() + maxDistToRoot[i];

        maxRadius.back() = std::max(maxRadius.back(), distances[i]);
      }

      // Compute AABB
488
      Configuration_t q (neutralConfiguration());
Joseph Mirabel's avatar
Joseph Mirabel committed
489
490
491
492
493
494
495
496
      fcl::AABB aabb;
      for (std::size_t k = 0; k < rootIdxs.size(); ++k)
      {
        JointIndex i = rootIdxs[k];
        value_type radius = maxRadius[k];

        fcl::AABB aabb_subtree;
        AABBStep::run(m.joints[i],
497
            AABBStep::ArgsType (m, q, true, aabb_subtree));
Joseph Mirabel's avatar
Joseph Mirabel committed
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512

        // Move AABB
        fcl::rotate   (aabb_subtree, m.jointPlacements[i].rotation   ());
        fcl::translate(aabb_subtree, m.jointPlacements[i].translation());

        // Add radius
        aabb_subtree.min_.array() -= radius;
        aabb_subtree.max_.array() += radius;
        
        // Merge back into previous one.
        if (k == 0) aabb  = aabb_subtree;
        else        aabb += aabb_subtree;
      }
      return aabb;
    }
513
514
515
516

    void replaceGeometryByConvexHull (GeomModel& gmodel,
        const std::vector<std::string>& gnames)
    {
517
#if HPP_FCL_VERSION_AT_LEAST(1,4,5)
518
519
520
521
522
523
524
525
526
527
528
529
530
531
      for (std::size_t i = 0; i < gnames.size(); ++i) {
        if (!gmodel.existGeometryName(gnames[i]))
          throw std::invalid_argument("Geometry " + gnames[i] + " does not "
              "exist.");
        GeomIndex gid = gmodel.getGeometryId(gnames[i]);
        GeometryObject& go = gmodel.geometryObjects[gid];
        if (go.geometry->getObjectType() == fcl::OT_BVH) {
          boost::shared_ptr<fcl::BVHModelBase> bvh =
            HPP_DYNAMIC_PTR_CAST(fcl::BVHModelBase, go.geometry);
          assert(bvh);
          bvh->buildConvexHull(false, "Qx");
          go.geometry = bvh->convex;
        }
      }
532
533
534
535
#else
      throw std::logic_error("hpp-fcl version is below 1.4.5 does not support "
          "the generation of convex hulls.");
#endif
536
    }
537
538
539

#if __cplusplus > 201103L
    using boost::serialization::make_nvp;
540
    using hpp::serialization::archive_device_wrapper;
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555

    template<typename To, typename Ti, typename UnaryOp>
    inline std::vector<To> serialize_to (const std::vector<Ti>& in, UnaryOp op)
    {
      std::vector<To> out;
      out.reserve(in.size());
      std::transform(in.begin(), in.end(), out.begin(), op);
      return out;
    }

    template<class Archive>
    void Device::save(Archive & ar, const unsigned int version) const
    {
      (void) version;

556
      archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
557
      ar & BOOST_SERIALIZATION_NVP(name_);
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
      bool written = (adw != NULL);
      ar & BOOST_SERIALIZATION_NVP(written);
      if (written) {
        // AbstractDevice
        ar & BOOST_SERIALIZATION_NVP(model_);
        //ar & BOOST_SERIALIZATION_NVP(geomModel_);

        // Device
        ar & BOOST_SERIALIZATION_NVP(name_);
        // - grippers_
        std::vector<FrameIndex> grippers;
        std::transform(grippers_.begin(), grippers_.end(), grippers.begin(),
            [](const GripperPtr_t& g) -> FrameIndex { return g->frameId(); });
        ar & BOOST_SERIALIZATION_NVP(grippers);
        ar & BOOST_SERIALIZATION_NVP(jointConstraints_);
        ar & BOOST_SERIALIZATION_NVP(weakPtr_);

        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);

        size_type nbDeviceData = numberDeviceData();
        ar & BOOST_SERIALIZATION_NVP(nbDeviceData);
      }
582
583
584
585
586
587
588
    }

    template<class Archive>
    void Device::load(Archive & ar, const unsigned int version)
    {
      (void) version;
      ar & BOOST_SERIALIZATION_NVP(name_);
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
      bool written;
      ar & BOOST_SERIALIZATION_NVP(written);

      archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
      if (written) {
        // AbstractDevice
        ar & BOOST_SERIALIZATION_NVP(model_);
        //ar & BOOST_SERIALIZATION_NVP(geomModel_);

        // Device
        ar & BOOST_SERIALIZATION_NVP(name_);
        // - grippers_
        std::vector<FrameIndex> grippers;
        ar & BOOST_SERIALIZATION_NVP(grippers);
        ar & BOOST_SERIALIZATION_NVP(jointConstraints_);
        ar & BOOST_SERIALIZATION_NVP(weakPtr_);

        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
        ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);

        size_type nbDeviceData;
        ar & BOOST_SERIALIZATION_NVP(nbDeviceData);

        if (!adw) { // if archive_device_wrapper, then this device will be
          // thrown away. No need to initialize it cleanly.
          grippers_.reserve(grippers.size());
          std::transform(grippers.begin(), grippers.end(), grippers_.begin(),
              [this](FrameIndex i) -> GripperPtr_t {
              return Gripper::create (model_->frames[i].name, weakPtr_);
              });
          createData();
          createGeomData();
          numberDeviceData(nbDeviceData);
        }
      } else if (!adw) // && !written
        throw std::logic_error("This archive does not contain a valid Device "
            "and the archive is not of type archive_device_wrapper.");
      // else TODO if (adw->device->name() != name_) ?
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
    }
#else
    template<class Archive>
    void Device::save(Archive &, const unsigned int) const
    {
      throw std::logic_error("Not implemented without C++ 11.");
    }

    template<class Archive>
    void Device::load(Archive &, const unsigned int)
    {
      throw std::logic_error("Not implemented without C++ 11.");
    }
#endif

    HPP_SERIALIZATION_SPLIT_IMPLEMENT(Device);
644
645
  } // namespace pinocchio
} // namespace hpp
646
647
648
649
650
651
652
653
654
655
656
657
658
659

namespace boost {
namespace serialization {
template<class Archive>
void serialize (Archive & ar, hpp::pinocchio::Device::JointLinearConstraint& c, const unsigned int version)
{
  (void) version;
  ar & make_nvp("offset", c.offset);
  ar & make_nvp("multiplier", c.multiplier);
  ar & make_nvp("joint", c.joint);
  ar & make_nvp("reference", c.reference);
}
}
}