device.hh 16.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//
// Copyright (c) 2016 CNRS
// Author: NMansard from Florent Lamiraux
//
//
// 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/>.

#ifndef HPP_PINOCCHIO_DEVICE_HH
#define HPP_PINOCCHIO_DEVICE_HH

# include <iostream>
# include <vector>
# include <list>

# include <hpp/util/debug.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
28

29
30
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/config.hh>
31
# include <hpp/pinocchio/deprecated.hh>
32
# include <hpp/pinocchio/extra-config-space.hh>
33
# include <hpp/pinocchio/device-object-vector.hh>
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

namespace hpp {
  namespace pinocchio {
    /// \brief Robot with geometric and dynamic pinocchio

    /// The creation of the device is done by Device::create(const
    /// std::string name).  This function returns a shared pointer
    /// to the newly created object.  \sa Smart pointers
    /// documentation:
    /// http://www.boost.org/libs/smart_ptr/smart_ptr.htm
    class HPP_PINOCCHIO_DLLAPI Device
    {
      friend class Joint;
    public:
      /// Flags to select computation
      /// To optimize computation time, computations performed by method
      /// computeForwardKinematics can be selected by calling method
      /// controlComputation.
      enum Computation_t {
53
	JOINT_POSITION = 0x1,
Joseph Mirabel's avatar
Joseph Mirabel committed
54
55
56
57
58
	JACOBIAN       = 0x2,
	VELOCITY       = 0x4,
	ACCELERATION   = 0x8,
	COM            = 0x10,
	ALL            = 0Xffff
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
      };

      /// Collision pairs between bodies
      typedef std::pair <JointPtr_t, JointPtr_t> CollisionPair_t;
      typedef std::list <CollisionPair_t> CollisionPairs_t;

      // -----------------------------------------------------------------------
      /// \name Construction, copy and destruction
      /// \{
      virtual ~Device() {};

      /// \brief Clone as a CkwsDevice
      /// The pinocchio model is not copied (only copy the pointer).
      /// A new Pinocchio "data" is created.
      /// As the model is not copied, cloning is a non constant operation. \sa cloneConst
Joseph Mirabel's avatar
Joseph Mirabel committed
74
      virtual DevicePtr_t clone() { return createCopy(weakPtr_.lock()); }
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
      /// \brief Clone as a CkwsDevice
      /// Both pinocchio objects model and data are copied.
      /// TODO: this method is not implemented yet (assert if called)
      DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
 
      /// Get name of device
      const std::string& name () const {return name_;}

      /// \brief Creation of a new device
      /// \return a shared pointer to the new device
      /// \param name Name of the device
      static DevicePtr_t create (const std::string & name);

      /// \brief Copy of a device
      /// \return A shared pointer to new device.
      /// \param device Device to be copied.
      /// The pinocchio model is not copied (only copy the pointer).
      /// A new Pinocchio "data" is created.
      static DevicePtr_t createCopy (const DevicePtr_t& device);
      static DevicePtr_t createCopyConst (const DeviceConstPtr_t& device);

      /// Set pinocchio model.
Nicolas Mansard's avatar
Nicolas Mansard committed
97
      void model( ModelPtr_t modelPtr ) { model_ = modelPtr; }
98
      /// Access to pinocchio model
99
      ModelConstPtr_t   modelPtr() const { return model_; }
100
      /// Access to pinocchio model
101
102
      ModelPtr_t        modelPtr()       { return model_; }
      /// Access to pinocchio model
103
      const Model& model()    const { assert(model_); return *model_; }
104
      /// Access to pinocchio model
105
      Model&       model()          { assert(model_); return *model_; }
106

107
108
109
      /// Set pinocchio geom.
      void geomModel( GeomModelPtr_t geomModelPtr ) { geomModel_ = geomModelPtr; }
      /// Access to pinocchio geomModel
110
111
112
113
      GeomModelConstPtr_t        geomModelPtr() const { return geomModel_; }
      /// Access to pinocchio geomModel
      GeomModelPtr_t             geomModelPtr() { return geomModel_; }
      /// Access to pinocchio geomModel
114
      const GeomModel & geomModel() const { assert(geomModel_); return *geomModel_; }
115
      /// Access to pinocchio geomModel
116
      GeomModel &       geomModel() { assert(geomModel_); return *geomModel_; }
117

118
      /// Set Pinocchio data corresponding to model
Nicolas Mansard's avatar
Nicolas Mansard committed
119
      void data( DataPtr_t dataPtr ) { data_ = dataPtr; resizeState(); }
120
      /// Access to Pinocchio data/
121
      DataConstPtr_t    dataPtr() const { return data_; }
122
      /// Access to Pinocchio data/
123
124
      DataPtr_t         dataPtr() { return data_; }
      /// Access to Pinocchio data/
125
      const Data & data() const { assert(data_); return *data_; }
126
      /// Access to Pinocchio data/
127
      Data &       data() { assert(data_); return *data_; }
128
129
130
      /// Create Pinocchio data from model.
      void createData();

131
132
133
      /// Set Pinocchio geomData corresponding to model
      void geomData( GeomDataPtr_t geomDataPtr ) { geomData_ = geomDataPtr; resizeState(); }
      /// Access to Pinocchio geomData/
134
135
136
137
      GeomDataConstPtr_t       geomDataPtr() const { return geomData_; }
      /// Access to Pinocchio geomData/
      GeomDataPtr_t            geomDataPtr()       { return geomData_; }
      /// Access to Pinocchio geomData/
138
      const GeomData& geomData() const    { assert(geomData_); return *geomData_; }
139
      /// Access to Pinocchio geomData/
140
      GeomData&       geomData()          { assert(geomData_); return *geomData_; }
141
142
143
      /// Create Pinocchio geomData from model.
      void createGeomData();

144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
      /// \}
      // -----------------------------------------------------------------------
      /// \name Joints
      /// \{

      //DEPREC /// Set the root of the kinematic chain
      //DEPREC virtual void rootJoint (JointPtr_t joint);
      //DEPREC /// Set position of root joint in world frame
      //DEPREC void rootJointPosition (const Transform3f& position);

      /// Get root joint
      JointPtr_t rootJoint () const;

      //DEPREC /// Register joint in internal containers
      //DEPREC void registerJoint (const JointPtr_t& joint);
159
160
      /// Get vector of joints
      inline const JointVector& getJointVector () const { return jointVector_; }
161
      inline JointVector& getJointVector () { return jointVector_; }
162
163

      /// Get the joint at configuration rank r
Florent Lamiraux's avatar
Florent Lamiraux committed
164
165
      /// \return  joint j such that j->rankInConfiguration () <=
      ///          r < j->rankInConfiguration () + j->configSize ()
166
167
168
      JointPtr_t getJointAtConfigRank (const size_type& r) const;

      /// Get the joint at velocity rank r
Florent Lamiraux's avatar
Florent Lamiraux committed
169
170
      /// \return  joint j such that j->rankInVelocity () <=
      ///          r < j->rankInVelocity () + j->numberDof ()
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
      JointPtr_t getJointAtVelocityRank (const size_type& r) const;

      /// Get joint by name
      /// \param name name of the joint.
      /// \throw runtime_error if device has no joint with this name
      JointPtr_t getJointByName (const std::string& name) const;

      /// Get joint by body name
      /// \throw runtime_error if device has no body with this name
      JointPtr_t getJointByBodyName (const std::string& name) const;

      /// Size of configuration vectors
      /// Sum of joint dimensions and of extra configuration space dimension
      size_type configSize () const;

      /// Size of velocity vectors
      /// Sum of joint number of degrees of freedom and of extra configuration
      /// space dimension
      size_type numberDof () const;

      /// \}
      // -----------------------------------------------------------------------
      /// \name Extra configuration space
      /// \{

      /// Get degrees of freedom to store internal values in configurations
      ///
      /// In some applications, it is useful to store extra variables with
      /// the configuration vector. For instance, when planning motions in
      /// state space using roadmap based methods, the velocity of the robot
      /// is stored in the nodes of the roadmap.
Nicolas Mansard's avatar
Nicolas Mansard committed
202
      ExtraConfigSpace& extraConfigSpace () { return extraConfigSpace_; }
203
204
205
206
207
208
209

      /// Get degrees of freedom to store internal values in configurations
      ///
      /// In some applications, it is useful to store extra variables with
      /// the configuration vector. For instance, when planning motions in
      /// state space using roadmap based methods, the velocity of the robot
      /// is stored in the nodes of the roadmap.
Nicolas Mansard's avatar
Nicolas Mansard committed
210
      const ExtraConfigSpace& extraConfigSpace () const { return extraConfigSpace_; }
211
212
213
214
215

      /// Set dimension of extra configuration space
      virtual void setDimensionExtraConfigSpace (const size_type& dimension)
      {
	extraConfigSpace_.setDimension (dimension);
Nicolas Mansard's avatar
Nicolas Mansard committed
216
	resizeState ();
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
      }

      /// \}
      // -----------------------------------------------------------------------
      /// \name Current state
      /// \{

      /// Get current configuration
      const Configuration_t& currentConfiguration () const
      {
	return currentConfiguration_;
      }
      /// Set current configuration
      /// \return True if the current configuration was modified and false if
      ///         the current configuration did not change.
Nicolas Mansard's avatar
Nicolas Mansard committed
232
233
      virtual bool currentConfiguration (ConfigurationIn_t configuration);

234
      /// Get the neutral configuration
235
      Configuration_t neutralConfiguration () const;
236
237
238
239
240
241
242
243
244
245

      /// Get current velocity
      const vector_t& currentVelocity () const
      {
	return currentVelocity_;
      }

      /// Set current velocity
      void currentVelocity (vectorIn_t velocity)
      {
246
        invalidate();
247
248
249
250
251
252
253
254
255
256
257
258
	currentVelocity_ = velocity;
      }

      /// Get current acceleration
      const vector_t& currentAcceleration () const
      {
	return currentAcceleration_;
      }

      /// Set current acceleration
      void currentAcceleration (vectorIn_t acceleration)
      {
259
        invalidate();
260
261
262
263
264
265
266
267
268
269
270
	currentAcceleration_ = acceleration;
      }
      /// \}
      // -----------------------------------------------------------------------
      /// \name Mass and center of mass
      /// \{

      /// Get mass of robot
      const value_type& mass () const;

      /// Get position of center of mass
271
      const vector3_t& positionCenterOfMass () const;
272
273
274
275
276
277

      /// Get Jacobian of center of mass with respect to configuration
      const ComJacobian_t& jacobianCenterOfMass () const;

      /// \}
      // -----------------------------------------------------------------------
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
      /// \name Grippers
      /// \{

      /// Add a gripper to the Device
      void addGripper (const GripperPtr_t& gripper)
      {
	grippers_.push_back (gripper);
      }

      /// Return list of grippers of the Device
      Grippers_t& grippers ()
      {
	return grippers_;
      }

      /// Return list of grippers of the Device
      const Grippers_t& grippers () const
      {
	return grippers_;
      }

      /// \}
300
      // -----------------------------------------------------------------------
301
302
303
304
      /// \name Collision and distance computation
      /// \{

      /// Get list of obstacles
305
306
307
308
309
310
311
312
313
314
315
      const ObjectVector_t& obstacles () const
      {
        return obstacles_;
      }

      /// Get list of obstacles
      /// \deprecated Use Device::obstacles() const
      const ObjectVector_t& obstacles (Request_t /*type*/) const HPP_PINOCCHIO_DEPRECATED
      {
        return obstacles();
      }
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342

      //DEPREC /// Add collision pairs between objects attached to two joints
      //DEPREC ///
      //DEPREC /// \param joint1 first joint
      //DEPREC /// \param joint2 second joint
      //DEPREC /// \param type collision or distance.
      //DEPREC ///
      //DEPREC /// Define collision pair between each object of joint 1 body and
      //DEPREC /// each object of joint2 body.
      //DEPREC virtual void addCollisionPairs (const JointPtr_t& joint1,
      //DEPREC 				      const JointPtr_t& joint2,
      //DEPREC 				      Request_t type);

      //DEPREC /// Remove collision pairs between objects attached to two joints
      //DEPREC ///
      //DEPREC /// \param joint1 first joint
      //DEPREC /// \param joint2 second joint
      //DEPREC /// \param type collision or distance.
      //DEPREC ///
      //DEPREC /// remove collision between each object of joint 1 body and
      //DEPREC /// each object of joint2 body
      //DEPREC virtual void removeCollisionPairs(const JointPtr_t& joint1,
      //DEPREC                                   const JointPtr_t& joint2,
      //DEPREC 				        Request_t type);

      /// Get list of collision or distance pairs
      /// \param type collision or distance.
343
      //DEPREC const CollisionPairs_t& collisionPairs (Request_t type) const;
344

Joseph Mirabel's avatar
Joseph Mirabel committed
345
      /// Vector of inner objects of the device
346
347
      DeviceObjectVector& objectVector () {return objectVector_; }
      const DeviceObjectVector& objectVector () const { return objectVector_; }
348
349
350
351
352
353
354
355
356
357
358
359
360

      /// Test collision of current configuration
      /// \param stopAtFirstCollision act as named
      /// \warning Users should call computeForwardKinematics first.
      bool collisionTest (const bool stopAtFirstCollision=true);

      /// Compute distances between pairs of objects stored in bodies
      /// \warning Users should call computeForwardKinematics first.
      void computeDistances ();

      /// Get result of distance computations
      const DistanceResults_t& distanceResults () const;
      /// \}
361
362
363
364
365
366
367
368
369
370
      // -----------------------------------------------------------------------
      /// \name Forward kinematics
      /// \{

      /// Select computation
      /// Optimize computation time by selecting only necessary values in
      /// method computeForwardKinematics.
      void controlComputation (const Computation_t& flag)
      {
	computationFlag_ = flag;
371
        invalidate();
372
373
374
375
376
377
378
      }
      /// Get computation flag
      Computation_t computationFlag () const
      {
	return computationFlag_;
      }
      /// Compute forward kinematics
379
380
381
      void computeForwardKinematics ();
      /// Update the geometry placement to the currentConfiguration
      void updateGeometryPlacements ();
382
383
384
385
386
387
388
389
390
391
      /// \}
      // -----------------------------------------------------------------------

      /// Print object in a stream
      virtual std::ostream& print (std::ostream& os) const;

    protected:
      /// \brief Constructor
      Device(const std::string& name);

Joseph Mirabel's avatar
Joseph Mirabel committed
392
393
394
      /// \brief Initialization.
      ///
      void init(const DeviceWkPtr_t& weakPtr);
395
396
397
      /// \brief Initialization of of a clone device.
      ///
      void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
398

399
400
      //DEPREC /// Recompute the number of distance pairs and resize the vector of distance results.
      //DEPREC void updateDistances ();
401
402
403
404
405

      /// \brief Copy Constructor
      Device(const Device& device);

    private:
406
407
408
409
410
      //DEPREC void computeJointPositions ();
      //DEPREC void computeJointJacobians ();
      //DEPREC void computeMass ();
      //DEPREC void computePositionCenterOfMass ();
      //DEPREC void computeJacobianCenterOfMass ();
Nicolas Mansard's avatar
Nicolas Mansard committed
411
412
413

      /// Resize configuration when changing data or extra-config.
      void resizeState ();
414
415
416
417
418

    protected:
      // Pinocchio objects
      ModelPtr_t model_; 
      DataPtr_t data_;
419
420
      GeomModelPtr_t geomModel_;
      GeomDataPtr_t geomData_;
421

422
423
      inline void invalidate () { upToDate_ = false; geomUpToDate_ = false; }

424
      std::string name_;
425
      //DEPREC DistanceResults_t distances_;
426
      //DEPREC JointByName_t jointByName_;
427
      JointVector jointVector_; // fake container with iterator mimicking hpp::model::JointVector_t
428
429
430
431
432
433
434
435
436
437
438
      //DEPREC JointVector_t jointByConfigRank_;
      //DEPREC JointVector_t jointByVelocityRank_;
      //DEPREC JointPtr_t rootJoint_;
      //DEPREC size_type numberDof_;
      //DEPREC size_type configSize_;
      Configuration_t currentConfiguration_;
      vector_t currentVelocity_;
      vector_t currentAcceleration_;
      //DEPREC vector3_t com_;
      //DEPREC ComJacobian_t jacobianCom_;
      //DEPREC value_type mass_;
439
      bool upToDate_, geomUpToDate_;
440
      Computation_t computationFlag_;
441
442
443
      //DEPREC // Collision pairs between bodies
      //DEPREC CollisionPairs_t collisionPairs_;
      //DEPREC CollisionPairs_t distancePairs_;
444
      //DEPREC // Obstacles
445
      ObjectVector_t obstacles_;
446
447
      //DEPREC ObjectVector_t collisionObstacles_;
      //DEPREC ObjectVector_t distanceObstacles_;
448
      DeviceObjectVector objectVector_;
449
450
      // Grippers
      Grippers_t grippers_;
451
452
453
      // Extra configuration space
      ExtraConfigSpace extraConfigSpace_;
      DeviceWkPtr_t weakPtr_;
454

455
456
    }; // class Device

457
458
459
    inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
    { return device.print(os); }

460
461
462
463
  } // namespace pinocchio
} // namespace hpp

#endif // HPP_PINOCCHIO_DEVICE_HH