hpp-pinocchio  4.12.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 // This file is part of hpp-pinocchio
7 // hpp-pinocchio is free software: you can redistribute it
8 // and/or modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation, either version
10 // 3 of the License, or (at your option) any later version.
11 //
12 // hpp-pinocchio is distributed in the hope that it will be
13 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Lesser Public License for more details. You should have
16 // received a copy of the GNU Lesser General Public License along with
17 // hpp-pinocchio If not, see
18 // <http://www.gnu.org/licenses/>.
19 
20 #ifndef HPP_PINOCCHIO_DEVICE_HH
21 #define HPP_PINOCCHIO_DEVICE_HH
22 
23 # include <iostream>
24 # include <vector>
25 # include <list>
26 
27 # include <boost/thread/mutex.hpp>
28 # include <boost/thread/condition_variable.hpp>
29 
30 # include <hpp/util/debug.hh>
31 # include <hpp/util/serialization-fwd.hh>
32 
33 # include <hpp/pinocchio/fwd.hh>
34 # include <hpp/pinocchio/frame.hh>
35 # include <hpp/pinocchio/pool.hh>
36 # include <hpp/pinocchio/config.hh>
41 
42 namespace hpp {
43  namespace pinocchio {
44 
46 
53  {
54  friend class Joint;
55  friend class Frame;
56  friend class DeviceSync;
57  friend class CollisionObject;
59  public:
61  typedef std::pair <JointPtr_t, JointPtr_t> CollisionPair_t;
62  typedef std::list <CollisionPair_t> CollisionPairs_t;
63 
64  // -----------------------------------------------------------------------
67  virtual ~Device();
68 
73  virtual DevicePtr_t clone() const { return createCopy(weakPtr_.lock()); }
77  DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
78 
79  DevicePtr_t self() const { return weakPtr_.lock(); }
80 
82  const std::string& name () const {return name_;}
83 
87  static DevicePtr_t create (const std::string & name);
88 
94  static DevicePtr_t createCopy (const DevicePtr_t& device);
96 
98  // -----------------------------------------------------------------------
101 
103  void setModel( ModelPtr_t modelPtr ) { model_ = modelPtr; }
104 
106  void setGeomModel( GeomModelPtr_t geomModelPtr ) { geomModel_ = geomModelPtr; }
107 
109  void setData( DataPtr_t dataPtr ) { d().data_ = dataPtr; resizeState(); }
111  void createData();
112 
114  void setGeomData( GeomDataPtr_t geomDataPtr ) { d().geomData_ = geomDataPtr; resizeState(); }
117 
119  virtual void removeJoints(const std::vector<std::string>& jointNames,
120  Configuration_t referenceConfig);
121 
123  // -----------------------------------------------------------------------
126 
134  {
136  JointPtr_t joint, reference;
137  };
138 
141 
143  Frame rootFrame () const;
144 
146  size_type nbJoints () const;
147 
149  JointPtr_t jointAt (const size_type& i) const;
150 
155 
160 
164  JointPtr_t getJointByName (const std::string& name) const;
165 
168  JointPtr_t getJointByBodyName (const std::string& name) const;
169 
173  Frame getFrameByName (const std::string& name) const;
174 
178 
183 
185  const LiegroupSpacePtr_t& configSpace () const { return configSpace_; }
186 
188  const LiegroupSpacePtr_t& RnxSOnConfigSpace () const { return configSpaceRnxSOn_; }
189 
192 
195 
196  const std::vector<JointLinearConstraint>& jointConstraints () const
197  {
198  return jointConstraints_;
199  }
200 
202  // -----------------------------------------------------------------------
205 
212  ExtraConfigSpace& extraConfigSpace () { return extraConfigSpace_; }
213 
220  const ExtraConfigSpace& extraConfigSpace () const { return extraConfigSpace_; }
221 
223  virtual void setDimensionExtraConfigSpace (const size_type& dimension)
224  {
225  extraConfigSpace_.setDimension (dimension);
226  resizeState ();
227  }
228 
230  // -----------------------------------------------------------------------
233 
235  void addGripper (const GripperPtr_t& gripper)
236  {
237  grippers_.push_back (gripper);
238  }
239 
242  {
243  return grippers_;
244  }
245 
247  const Grippers_t& grippers () const
248  {
249  return grippers_;
250  }
251 
253  // -----------------------------------------------------------------------
256 
261 
264 
268 
272  bool collisionTest (const bool stopAtFirstCollision=true);
273 
277 
281  // -----------------------------------------------------------------------
285 
287  void numberDeviceData (const size_type& s);
288 
291 
293  // -----------------------------------------------------------------------
294 
296  virtual std::ostream& print (std::ostream& os) const;
297 
306  fcl::AABB computeAABB() const;
307 
308  void controlComputation (const Computation_t& flag);
309 
310  protected:
312  Device(const std::string& name);
313 
316  void init(const DeviceWkPtr_t& weakPtr);
319  void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
320 
322  Device(const Device& device);
323 
324  private:
325 
327  void resizeState ();
328 
329  protected:
331 
332  DeviceData & d () { return d_; }
333  DeviceData const& d () const { return d_; }
334 
335  inline void invalidate () { d_.invalidate(); }
336 
337  std::string name_;
338  // Grippers
340  LiegroupSpacePtr_t configSpace_, configSpaceRnxSOn_;
341  // Extra configuration space
343  // Joint linear constraints
344  std::vector<JointLinearConstraint> jointConstraints_;
345  DeviceWkPtr_t weakPtr_;
346 
347  private:
348  Pool<DeviceData> datas_;
349 
350  protected:
351  Device() {}
352  private:
353  HPP_SERIALIZABLE_SPLIT();
354  }; // class Device
355 
357  const std::vector<std::string>& geometryNames);
358 
359  inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
360  { return device.print(os); }
361 
362  } // namespace pinocchio
363 } // namespace hpp
364 
365 BOOST_CLASS_EXPORT_KEY(hpp::pinocchio::Device)
366 
367 #endif // HPP_PINOCCHIO_DEVICE_HH
Abstract class representing a Device.
Definition: device-sync.hh:30
Definition: center-of-mass-computation.hh:44
Definition: collision-object.hh:44
Definition: device-sync.hh:163
Robot with geometric and dynamic pinocchio.
Definition: device.hh:53
const LiegroupSpacePtr_t & configSpace() const
Returns a LiegroupSpace representing the configuration space.
Definition: device.hh:185
JointPtr_t getJointByName(const std::string &name) const
Configuration_t neutralConfiguration() const
Get the neutral configuration.
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:77
CollisionObjectPtr_t objectAt(const size_type &i) const
void createGeomData()
Create Pinocchio geomData from model.
const std::string & name() const
Get name of device.
Definition: device.hh:82
void invalidate()
Definition: device.hh:335
Grippers_t & grippers()
Return list of grippers of the Device.
Definition: device.hh:241
DeviceData & d()
Definition: device.hh:332
Frame rootFrame() const
Get root frame.
fcl::AABB computeAABB() const
void addGripper(const GripperPtr_t &gripper)
Add a gripper to the Device.
Definition: device.hh:235
const Grippers_t & grippers() const
Return list of grippers of the Device.
Definition: device.hh:247
const DistanceResults_t & distanceResults() const
Get result of distance computations.
void setData(DataPtr_t dataPtr)
Set Pinocchio data corresponding to model.
Definition: device.hh:109
JointPtr_t getJointByBodyName(const std::string &name) const
void init(const DeviceWkPtr_t &weakPtr)
Initialization.
ExtraConfigSpace extraConfigSpace_
Definition: device.hh:342
std::vector< JointLinearConstraint > jointConstraints_
Definition: device.hh:344
size_type nbJoints() const
Get number of joints.
void setModel(ModelPtr_t modelPtr)
Set pinocchio model.
Definition: device.hh:103
DeviceData d_
Definition: device.hh:330
std::list< CollisionPair_t > CollisionPairs_t
Definition: device.hh:62
void setGeomModel(GeomModelPtr_t geomModelPtr)
Set pinocchio geom.
Definition: device.hh:106
void controlComputation(const Computation_t &flag)
ExtraConfigSpace & extraConfigSpace()
Definition: device.hh:212
size_type numberDof() const
void createData()
Create Pinocchio data from model.
Frame getFrameByName(const std::string &name) const
JointPtr_t getJointAtVelocityRank(const size_type &r) const
LiegroupSpacePtr_t configSpace_
Definition: device.hh:340
std::pair< JointPtr_t, JointPtr_t > CollisionPair_t
Collision pairs between bodies.
Definition: device.hh:61
Device()
Definition: device.hh:351
virtual void removeJoints(const std::vector< std::string > &jointNames, Configuration_t referenceConfig)
Remove some joints from the configuration space.
DeviceWkPtr_t weakPtr_
Definition: device.hh:345
BodyPtr_t obstacles() const
static DevicePtr_t createCopy(const DevicePtr_t &device)
Copy of a device.
JointPtr_t getJointAtConfigRank(const size_type &r) const
virtual void setDimensionExtraConfigSpace(const size_type &dimension)
Set dimension of extra configuration space.
Definition: device.hh:223
const ExtraConfigSpace & extraConfigSpace() const
Definition: device.hh:220
Grippers_t grippers_
Definition: device.hh:339
void numberDeviceData(const size_type &s)
Set the maximum number of concurrent use of the Device.
JointPtr_t jointAt(const size_type &i) const
Access i-th joint.
std::string name_
Definition: device.hh:337
bool collisionTest(const bool stopAtFirstCollision=true)
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:73
static DevicePtr_t createCopyConst(const DeviceConstPtr_t &device)
const LiegroupSpacePtr_t & RnxSOnConfigSpace() const
See Joint::RnxSOnConfigurationSpace.
Definition: device.hh:188
void setGeomData(GeomDataPtr_t geomDataPtr)
Set Pinocchio geomData corresponding to model.
Definition: device.hh:114
void addJointConstraint(JointLinearConstraint constraint)
Add a joint constraint.
size_type configSize() const
static DevicePtr_t create(const std::string &name)
Creation of a new device.
Device(const Device &device)
Copy Constructor.
size_type nbObjects() const
Number of objects.
void initCopy(const DeviceWkPtr_t &weakPtr, const Device &other)
Initialization of of a clone device.
JointPtr_t rootJoint() const
Get root joint.
size_type numberDeviceData() const
Get the number of DeviceData.
DeviceData const & d() const
Definition: device.hh:333
const std::vector< JointLinearConstraint > & jointConstraints() const
Definition: device.hh:196
Device(const std::string &name)
Constructor.
virtual std::ostream & print(std::ostream &os) const
Print object in a stream.
Definition: extra-config-space.hh:35
Robot frame.
Definition: frame.hh:31
Definition: joint.hh:47
Pool of objects.
Definition: pool.hh:49
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
std::vector< fcl::DistanceResult > DistanceResults_t
Definition: fwd.hh:108
Computation_t
Definition: device-data.hh:30
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:111
shared_ptr< GeomModel > GeomModelPtr_t
Definition: fwd.hh:121
void replaceGeometryByConvexHull(GeomModel &geomModel, const std::vector< std::string > &geometryNames)
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:359
shared_ptr< GeomData > GeomDataPtr_t
Definition: fwd.hh:123
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
matrix_t::Index size_type
Definition: fwd.hh:84
shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:96
double value_type
Definition: fwd.hh:40
vector_t Configuration_t
Definition: fwd.hh:76
std::vector< GripperPtr_t > Grippers_t
Definition: fwd.hh:114
::pinocchio::GeometryModel GeomModel
Definition: fwd.hh:67
shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:107
shared_ptr< Gripper > GripperPtr_t
Definition: fwd.hh:113
shared_ptr< Model > ModelPtr_t
Definition: fwd.hh:116
shared_ptr< CollisionObject > CollisionObjectPtr_t
Definition: fwd.hh:104
shared_ptr< Data > DataPtr_t
Definition: fwd.hh:118
Utility functions.
Definition: body.hh:30
Definition: collision-object.hh:32
Definition: device-data.hh:42
void invalidate()
Definition: device-data.hh:46
JointPtr_t joint
Definition: device.hh:136
value_type multiplier
Definition: device.hh:135