hpp-pinocchio  4.13.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
device.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016 CNRS
3 // Author: NMansard from Florent Lamiraux
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 #ifndef HPP_PINOCCHIO_DEVICE_HH
32 #define HPP_PINOCCHIO_DEVICE_HH
33 
34 #include <boost/thread/condition_variable.hpp>
35 #include <boost/thread/mutex.hpp>
36 #include <hpp/pinocchio/config.hh>
41 #include <hpp/pinocchio/frame.hh>
42 #include <hpp/pinocchio/fwd.hh>
43 #include <hpp/pinocchio/pool.hh>
44 #include <hpp/util/debug.hh>
45 #include <hpp/util/serialization-fwd.hh>
46 #include <iostream>
47 #include <list>
48 #include <vector>
49 
50 namespace hpp {
51 namespace pinocchio {
52 
54 
61  friend class Joint;
62  friend class Frame;
63  friend class DeviceSync;
64  friend class CollisionObject;
66 
67  public:
69  typedef std::pair<JointPtr_t, JointPtr_t> CollisionPair_t;
70  typedef std::list<CollisionPair_t> CollisionPairs_t;
71 
72  // -----------------------------------------------------------------------
75  virtual ~Device();
76 
82  virtual DevicePtr_t clone() const { return createCopy(weakPtr_.lock()); }
86  DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
87 
88  DevicePtr_t self() const { return weakPtr_.lock(); }
89 
91  const std::string& name() const { return name_; }
92 
96  static DevicePtr_t create(const std::string& name);
97 
103  static DevicePtr_t createCopy(const DevicePtr_t& device);
104  static DevicePtr_t createCopyConst(const DeviceConstPtr_t& device);
105 
107  // -----------------------------------------------------------------------
110 
112  void setModel(ModelPtr_t modelPtr) { model_ = modelPtr; }
113 
115  void setGeomModel(GeomModelPtr_t geomModelPtr) { geomModel_ = geomModelPtr; }
116 
118  void setData(DataPtr_t dataPtr) {
119  d().data_ = dataPtr;
120  resizeState();
121  }
123  void createData();
124 
126  void setGeomData(GeomDataPtr_t geomDataPtr) {
127  d().geomData_ = geomDataPtr;
128  resizeState();
129  }
131  void createGeomData();
132 
134  virtual void removeJoints(const std::vector<std::string>& jointNames,
135  Configuration_t referenceConfig);
136 
138  // -----------------------------------------------------------------------
141 
149  value_type offset, multiplier;
151  };
152 
154  JointPtr_t rootJoint() const;
155 
157  Frame rootFrame() const;
158 
160  size_type nbJoints() const;
161 
163  JointPtr_t jointAt(const size_type& i) const;
164 
168  JointPtr_t getJointAtConfigRank(const size_type& r) const;
169 
173  JointPtr_t getJointAtVelocityRank(const size_type& r) const;
174 
178  JointPtr_t getJointByName(const std::string& name) const;
179 
182  JointPtr_t getJointByBodyName(const std::string& name) const;
183 
187  Frame getFrameByName(const std::string& name) const;
188 
191  size_type configSize() const;
192 
196  size_type numberDof() const;
197 
199  const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
200 
203  return configSpaceRnxSOn_;
204  }
205 
207  Configuration_t neutralConfiguration() const;
208 
210  void addJointConstraint(JointLinearConstraint constraint);
211 
212  const std::vector<JointLinearConstraint>& jointConstraints() const {
213  return jointConstraints_;
214  }
215 
217  // -----------------------------------------------------------------------
220 
227  ExtraConfigSpace& extraConfigSpace() { return extraConfigSpace_; }
228 
235  const ExtraConfigSpace& extraConfigSpace() const { return extraConfigSpace_; }
236 
238  virtual void setDimensionExtraConfigSpace(const size_type& dimension) {
239  extraConfigSpace_.setDimension(dimension);
240  resizeState();
241  }
242 
244  // -----------------------------------------------------------------------
247 
249  void addGripper(const GripperPtr_t& gripper) { grippers_.push_back(gripper); }
250 
252  Grippers_t& grippers() { return grippers_; }
253 
255  const Grippers_t& grippers() const { return grippers_; }
256 
258  // -----------------------------------------------------------------------
261 
265  BodyPtr_t obstacles() const;
266 
268  size_type nbObjects() const;
269 
272  CollisionObjectPtr_t objectAt(const size_type& i) const;
273 
277  bool collisionTest(const bool stopAtFirstCollision = true);
278 
281  void computeDistances();
282 
284  const DistanceResults_t& distanceResults() const;
286  // -----------------------------------------------------------------------
290 
292  void numberDeviceData(const size_type& s);
293 
295  size_type numberDeviceData() const;
296 
298  // -----------------------------------------------------------------------
299 
301  virtual std::ostream& print(std::ostream& os) const;
302 
311  fcl::AABB computeAABB() const;
312 
313  void controlComputation(const Computation_t& flag);
314 
315  protected:
317  Device(const std::string& name);
318 
321  void init(const DeviceWkPtr_t& weakPtr);
324  void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
325 
327  Device(const Device& device);
328 
329  private:
331  void resizeState();
332 
333  protected:
335 
336  DeviceData& d() { return d_; }
337  DeviceData const& d() const { return d_; }
338 
339  inline void invalidate() { d_.invalidate(); }
340 
341  std::string name_;
342  // Grippers
345  // Extra configuration space
347  // Joint linear constraints
348  std::vector<JointLinearConstraint> jointConstraints_;
349  DeviceWkPtr_t weakPtr_;
350 
351  private:
352  Pool<DeviceData> datas_;
353 
354  protected:
355  Device() {}
356 
357  private:
358  HPP_SERIALIZABLE_SPLIT();
359 }; // class Device
360 
361 void replaceGeometryByConvexHull(GeomModel& geomModel,
362  const std::vector<std::string>& geometryNames);
363 
364 inline std::ostream& operator<<(std::ostream& os,
365  const hpp::pinocchio::Device& device) {
366  return device.print(os);
367 }
368 
369 } // namespace pinocchio
370 } // namespace hpp
371 
372 BOOST_CLASS_EXPORT_KEY(hpp::pinocchio::Device)
373 
374 #endif // HPP_PINOCCHIO_DEVICE_HH
virtual DevicePtr_t clone() const
Clone as a CkwsDevice The pinocchio model is not copied (only copy the pointer). A new Pinocchio "dat...
Definition: device.hh:82
JointPtr_t reference
Definition: device.hh:150
shared_ptr< GeomModel > GeomModelPtr_t
Definition: fwd.hh:133
Definition: extra-config-space.hh:45
shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:108
Utility functions.
Definition: body.hh:39
std::vector< JointLinearConstraint > jointConstraints_
Definition: device.hh:348
shared_ptr< Data > DataPtr_t
Definition: fwd.hh:130
::pinocchio::GeometryModel GeomModel
Definition: fwd.hh:78
std::vector< fcl::DistanceResult > DistanceResults_t
Definition: fwd.hh:120
Definition: device-data.hh:51
Pool of objects.
Definition: pool.hh:57
ExtraConfigSpace & extraConfigSpace()
Definition: device.hh:227
const LiegroupSpacePtr_t & RnxSOnConfigSpace() const
See Joint::RnxSOnConfigurationSpace.
Definition: device.hh:202
void invalidate()
Definition: device.hh:339
Grippers_t grippers_
Definition: device.hh:343
Robot frame.
Definition: frame.hh:42
matrix_t::Index size_type
Definition: fwd.hh:96
virtual std::ostream & print(std::ostream &os) const
Print object in a stream.
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:118
const ExtraConfigSpace & extraConfigSpace() const
Definition: device.hh:235
DevicePtr_t cloneConst() const
Clone as a CkwsDevice Both pinocchio objects model and data are copied. TODO: this method is not impl...
Definition: device.hh:86
shared_ptr< GeomData > GeomDataPtr_t
Definition: fwd.hh:135
void setGeomData(GeomDataPtr_t geomDataPtr)
Set Pinocchio geomData corresponding to model.
Definition: device.hh:126
ExtraConfigSpace extraConfigSpace_
Definition: device.hh:346
DeviceData d_
Definition: device.hh:334
void replaceGeometryByConvexHull(GeomModel &geomModel, const std::vector< std::string > &geometryNames)
void addGripper(const GripperPtr_t &gripper)
Add a gripper to the Device.
Definition: device.hh:249
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:364
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
void setGeomModel(GeomModelPtr_t geomModelPtr)
Set pinocchio geom.
Definition: device.hh:115
shared_ptr< Gripper > GripperPtr_t
Definition: fwd.hh:125
DeviceData const & d() const
Definition: device.hh:337
Computation_t
Definition: device-data.hh:40
shared_ptr< Model > ModelPtr_t
Definition: fwd.hh:128
Abstract class representing a Device.
Definition: device-sync.hh:40
std::string name_
Definition: device.hh:341
Definition: center-of-mass-computation.hh:52
Device()
Definition: device.hh:355
void setModel(ModelPtr_t modelPtr)
Set pinocchio model.
Definition: device.hh:112
value_type offset
Definition: device.hh:149
Robot with geometric and dynamic pinocchio.
Definition: device.hh:60
Definition: collision-object.hh:52
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:123
shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:119
const LiegroupSpacePtr_t & configSpace() const
Returns a LiegroupSpace representing the configuration space.
Definition: device.hh:199
shared_ptr< CollisionObject > CollisionObjectPtr_t
Definition: fwd.hh:116
double value_type
Definition: fwd.hh:50
DeviceData & d()
Definition: device.hh:336
Definition: collision-object.hh:40
Grippers_t & grippers()
Return list of grippers of the Device.
Definition: device.hh:252
std::vector< GripperPtr_t > Grippers_t
Definition: fwd.hh:126
const Grippers_t & grippers() const
Return list of grippers of the Device.
Definition: device.hh:255
DeviceWkPtr_t weakPtr_
Definition: device.hh:349
virtual void setDimensionExtraConfigSpace(const size_type &dimension)
Set dimension of extra configuration space.
Definition: device.hh:238
const std::string & name() const
Get name of device.
Definition: device.hh:91
void setData(DataPtr_t dataPtr)
Set Pinocchio data corresponding to model.
Definition: device.hh:118
std::pair< JointPtr_t, JointPtr_t > CollisionPair_t
Collision pairs between bodies.
Definition: device.hh:69
vector_t Configuration_t
Definition: fwd.hh:88
Definition: device-sync.hh:204
Definition: joint.hh:57
const std::vector< JointLinearConstraint > & jointConstraints() const
Definition: device.hh:212
std::list< CollisionPair_t > CollisionPairs_t
Definition: device.hh:70
LiegroupSpacePtr_t configSpaceRnxSOn_
Definition: device.hh:344
void invalidate()
Definition: device-data.hh:55
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:150