hpp-pinocchio  5.1.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 
161  size_type nbJoints() const;
162 
167  JointPtr_t jointAt(const size_type& i) const;
168 
172  JointPtr_t getJointAtConfigRank(const size_type& r) const;
173 
177  JointPtr_t getJointAtVelocityRank(const size_type& r) const;
178 
182  JointPtr_t getJointByName(const std::string& name) const;
183 
186  JointPtr_t getJointByBodyName(const std::string& name) const;
187 
191  Frame getFrameByName(const std::string& name) const;
192 
195  size_type configSize() const;
196 
200  size_type numberDof() const;
201 
203  const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
204 
207  return configSpaceRnxSOn_;
208  }
209 
211  Configuration_t neutralConfiguration() const;
212 
214  void addJointConstraint(JointLinearConstraint constraint);
215 
216  const std::vector<JointLinearConstraint>& jointConstraints() const {
217  return jointConstraints_;
218  }
219 
221  // -----------------------------------------------------------------------
224 
231  ExtraConfigSpace& extraConfigSpace() { return extraConfigSpace_; }
232 
239  const ExtraConfigSpace& extraConfigSpace() const { return extraConfigSpace_; }
240 
242  virtual void setDimensionExtraConfigSpace(const size_type& dimension) {
243  extraConfigSpace_.setDimension(dimension);
244  resizeState();
245  }
246 
248  // -----------------------------------------------------------------------
251 
253  void addGripper(const GripperPtr_t& gripper) { grippers_.push_back(gripper); }
254 
256  Grippers_t& grippers() { return grippers_; }
257 
259  const Grippers_t& grippers() const { return grippers_; }
260 
262  // -----------------------------------------------------------------------
265 
269  BodyPtr_t obstacles() const;
270 
272  size_type nbObjects() const;
273 
276  CollisionObjectPtr_t objectAt(const size_type& i) const;
277 
281  bool collisionTest(const bool stopAtFirstCollision = true);
282 
285  void computeDistances();
286 
288  const DistanceResults_t& distanceResults() const;
290  // -----------------------------------------------------------------------
294 
296  void numberDeviceData(const size_type& s);
297 
299  size_type numberDeviceData() const;
300 
302  // -----------------------------------------------------------------------
303 
305  virtual std::ostream& print(std::ostream& os) const;
306 
315  fcl::AABB computeAABB() const;
316 
317  protected:
319  Device(const std::string& name);
320 
323  void init(const DeviceWkPtr_t& weakPtr);
326  void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
327 
329  Device(const Device& device);
330 
331  private:
333  void resizeState();
334 
335  protected:
337 
338  DeviceData& d() { return d_; }
339  DeviceData const& d() const { return d_; }
340 
341  inline void invalidate() { d_.invalidate(); }
342 
343  std::string name_;
344  // Grippers
347  // Extra configuration space
349  // Joint linear constraints
350  std::vector<JointLinearConstraint> jointConstraints_;
351  DeviceWkPtr_t weakPtr_;
352 
353  private:
354  Pool<DeviceData> datas_;
355 
356  protected:
357  Device() {}
358 
359  private:
360  HPP_SERIALIZABLE_SPLIT();
361 }; // class Device
362 
363 void replaceGeometryByConvexHull(GeomModel& geomModel,
364  const std::vector<std::string>& geometryNames);
365 
366 inline std::ostream& operator<<(std::ostream& os,
367  const hpp::pinocchio::Device& device) {
368  return device.print(os);
369 }
370 
371 } // namespace pinocchio
372 } // namespace hpp
373 
374 BOOST_CLASS_EXPORT_KEY(hpp::pinocchio::Device)
375 
376 #endif // HPP_PINOCCHIO_DEVICE_HH
hpp::pinocchio::Device::jointConstraints
const std::vector< JointLinearConstraint > & jointConstraints() const
Definition: device.hh:216
hpp::pinocchio::Device::CollisionPair_t
std::pair< JointPtr_t, JointPtr_t > CollisionPair_t
Collision pairs between bodies.
Definition: device.hh:69
hpp::pinocchio::DeviceSync
Definition: device-sync.hh:212
hpp::pinocchio::Grippers_t
std::vector< GripperPtr_t > Grippers_t
Definition: fwd.hh:126
device-sync.hh
hpp::pinocchio::Device
Robot with geometric and dynamic pinocchio.
Definition: device.hh:60
hpp::pinocchio::DevicePtr_t
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:118
hpp::pinocchio::Pool
Pool of objects.
Definition: pool.hh:57
hpp::pinocchio::value_type
double value_type
Definition: fwd.hh:51
hpp::pinocchio::Device::setGeomData
void setGeomData(GeomDataPtr_t geomDataPtr)
Set Pinocchio geomData corresponding to model.
Definition: device.hh:126
hpp::pinocchio::Device::jointConstraints_
std::vector< JointLinearConstraint > jointConstraints_
Definition: device.hh:350
hpp::pinocchio::Device::setGeomModel
void setGeomModel(GeomModelPtr_t geomModelPtr)
Set pinocchio geom.
Definition: device.hh:115
hpp::pinocchio::JointPtr_t
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:123
hpp::pinocchio::Device::JointLinearConstraint::offset
value_type offset
Definition: device.hh:149
hpp::pinocchio::Device::invalidate
void invalidate()
Definition: device.hh:341
hpp::pinocchio::CollisionObjectPtr_t
shared_ptr< CollisionObject > CollisionObjectPtr_t
Definition: fwd.hh:116
hpp::pinocchio::Device::grippers
const Grippers_t & grippers() const
Return list of grippers of the Device.
Definition: device.hh:259
hpp::pinocchio::Device::configSpace
const LiegroupSpacePtr_t & configSpace() const
Returns a LiegroupSpace representing the configuration space.
Definition: device.hh:203
hpp::pinocchio::Device::cloneConst
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
hpp::pinocchio::LiegroupSpacePtr_t
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:150
HPP_PINOCCHIO_DLLAPI
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:88
hpp::pinocchio::DeviceConstPtr_t
shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:119
hpp::pinocchio::ExtraConfigSpace
Definition: extra-config-space.hh:45
hpp::pinocchio::Device::d
DeviceData const & d() const
Definition: device.hh:339
hpp::pinocchio::Device::d_
DeviceData d_
Definition: device.hh:336
hpp::pinocchio::Device::configSpaceRnxSOn_
LiegroupSpacePtr_t configSpaceRnxSOn_
Definition: device.hh:346
hpp::pinocchio::Device::RnxSOnConfigSpace
const LiegroupSpacePtr_t & RnxSOnConfigSpace() const
See Joint::RnxSOnConfigurationSpace.
Definition: device.hh:206
fwd.hh
hpp::pinocchio::Joint
Definition: joint.hh:57
deprecated.hh
hpp::pinocchio::Device::JointLinearConstraint
Definition: device.hh:148
hpp::pinocchio::DeviceData
Definition: device-data.hh:51
hpp::pinocchio::Device::extraConfigSpace_
ExtraConfigSpace extraConfigSpace_
Definition: device.hh:348
hpp::pinocchio::Device::weakPtr_
DeviceWkPtr_t weakPtr_
Definition: device.hh:351
hpp::pinocchio::size_type
matrix_t::Index size_type
Definition: fwd.hh:97
hpp::pinocchio::ModelPtr_t
shared_ptr< Model > ModelPtr_t
Definition: fwd.hh:128
frame.hh
device-data.hh
hpp
Utility functions.
Definition: body.hh:39
hpp::pinocchio::DistanceResults_t
std::vector< fcl::DistanceResult > DistanceResults_t
Definition: fwd.hh:120
hpp::pinocchio::GeomModel
::pinocchio::GeometryModel GeomModel
Definition: fwd.hh:79
hpp::pinocchio::Device::Device
Device()
Definition: device.hh:357
hpp::pinocchio::Configuration_t
vector_t Configuration_t
Definition: fwd.hh:89
hpp::pinocchio::Device::name_
std::string name_
Definition: device.hh:343
hpp::pinocchio::Device::d
DeviceData & d()
Definition: device.hh:338
hpp::pinocchio::DataPtr_t
shared_ptr< Data > DataPtr_t
Definition: fwd.hh:130
hpp::pinocchio::replaceGeometryByConvexHull
void replaceGeometryByConvexHull(GeomModel &geomModel, const std::vector< std::string > &geometryNames)
hpp::pinocchio::GripperPtr_t
shared_ptr< Gripper > GripperPtr_t
Definition: fwd.hh:125
hpp::pinocchio::Device::extraConfigSpace
ExtraConfigSpace & extraConfigSpace()
Definition: device.hh:231
hpp::pinocchio::Device::extraConfigSpace
const ExtraConfigSpace & extraConfigSpace() const
Definition: device.hh:239
hpp::pinocchio::Device::grippers_
Grippers_t grippers_
Definition: device.hh:345
hpp::pinocchio::GeomModelPtr_t
shared_ptr< GeomModel > GeomModelPtr_t
Definition: fwd.hh:133
hpp::pinocchio::CenterOfMassComputation
Definition: center-of-mass-computation.hh:52
pool.hh
hpp::pinocchio::Device::JointLinearConstraint::reference
JointPtr_t reference
Definition: device.hh:150
hpp::pinocchio::Device::clone
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
hpp::pinocchio::BodyPtr_t
shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:109
extra-config-space.hh
hpp::pinocchio::AbstractDevice
Abstract class representing a Device.
Definition: device-sync.hh:41
hpp::pinocchio::Device::grippers
Grippers_t & grippers()
Return list of grippers of the Device.
Definition: device.hh:256
hpp::pinocchio::Device::CollisionPairs_t
std::list< CollisionPair_t > CollisionPairs_t
Definition: device.hh:70
hpp::pinocchio::Frame
Robot frame.
Definition: frame.hh:42
hpp::pinocchio::Device::setModel
void setModel(ModelPtr_t modelPtr)
Set pinocchio model.
Definition: device.hh:112
hpp::pinocchio::operator<<
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:366
hpp::pinocchio::GeomDataPtr_t
shared_ptr< GeomData > GeomDataPtr_t
Definition: fwd.hh:135
hpp::pinocchio::Device::addGripper
void addGripper(const GripperPtr_t &gripper)
Add a gripper to the Device.
Definition: device.hh:253
hpp::pinocchio::CollisionObject
Definition: collision-object.hh:52
hpp::pinocchio::Device::print
virtual std::ostream & print(std::ostream &os) const
Print object in a stream.
hpp::pinocchio::Device::setData
void setData(DataPtr_t dataPtr)
Set Pinocchio data corresponding to model.
Definition: device.hh:118
hpp::pinocchio::DeviceData::invalidate
void invalidate()
Definition: device-data.hh:55
config.hh
hpp::pinocchio::Device::name
const std::string & name() const
Get name of device.
Definition: device.hh:91
pinocchio
Definition: collision-object.hh:40
hpp::pinocchio::Device::setDimensionExtraConfigSpace
virtual void setDimensionExtraConfigSpace(const size_type &dimension)
Set dimension of extra configuration space.
Definition: device.hh:242