hpp-pinocchio  4.9.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 
32 # include <hpp/pinocchio/fwd.hh>
33 # include <hpp/pinocchio/frame.hh>
34 # include <hpp/pinocchio/pool.hh>
35 # include <hpp/pinocchio/config.hh>
40 
41 namespace hpp {
42  namespace pinocchio {
43 
45 
52  {
53  friend class Joint;
54  friend class Frame;
55  friend class DeviceSync;
56  friend class CollisionObject;
58  public:
60  typedef std::pair <JointPtr_t, JointPtr_t> CollisionPair_t;
61  typedef std::list <CollisionPair_t> CollisionPairs_t;
62 
63  // -----------------------------------------------------------------------
66  virtual ~Device();
67 
72  virtual DevicePtr_t clone() const { return createCopy(weakPtr_.lock()); }
76  DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
77 
79  const std::string& name () const {return name_;}
80 
84  static DevicePtr_t create (const std::string & name);
85 
91  static DevicePtr_t createCopy (const DevicePtr_t& device);
92  static DevicePtr_t createCopyConst (const DeviceConstPtr_t& device);
93 
95  // -----------------------------------------------------------------------
98 
100  void setModel( ModelPtr_t modelPtr ) { model_ = modelPtr; }
101 
103  void setGeomModel( GeomModelPtr_t geomModelPtr ) { geomModel_ = geomModelPtr; }
104 
106  void setData( DataPtr_t dataPtr ) { d().data_ = dataPtr; resizeState(); }
108  void createData();
109 
111  void setGeomData( GeomDataPtr_t geomDataPtr ) { d().geomData_ = geomDataPtr; resizeState(); }
113  void createGeomData();
114 
116  // -----------------------------------------------------------------------
119 
127  {
128  value_type offset, multiplier;
130  };
131 
133  JointPtr_t rootJoint () const;
134 
136  Frame rootFrame () const;
137 
139  size_type nbJoints () const;
140 
142  JointPtr_t jointAt (const size_type& i) const;
143 
147  JointPtr_t getJointAtConfigRank (const size_type& r) const;
148 
152  JointPtr_t getJointAtVelocityRank (const size_type& r) const;
153 
157  JointPtr_t getJointByName (const std::string& name) const;
158 
161  JointPtr_t getJointByBodyName (const std::string& name) const;
162 
166  Frame getFrameByName (const std::string& name) const;
167 
170  size_type configSize () const;
171 
175  size_type numberDof () const;
176 
178  const LiegroupSpacePtr_t& configSpace () const { return configSpace_; }
179 
181  const LiegroupSpacePtr_t& RnxSOnConfigSpace () const { return configSpaceRnxSOn_; }
182 
184  Configuration_t neutralConfiguration () const;
185 
187  void addJointConstraint (JointLinearConstraint constraint);
188 
189  const std::vector<JointLinearConstraint>& jointConstraints () const
190  {
191  return jointConstraints_;
192  }
193 
195  // -----------------------------------------------------------------------
198 
205  ExtraConfigSpace& extraConfigSpace () { return extraConfigSpace_; }
206 
213  const ExtraConfigSpace& extraConfigSpace () const { return extraConfigSpace_; }
214 
216  virtual void setDimensionExtraConfigSpace (const size_type& dimension)
217  {
218  extraConfigSpace_.setDimension (dimension);
219  resizeState ();
220  }
221 
223  // -----------------------------------------------------------------------
226 
228  void addGripper (const GripperPtr_t& gripper)
229  {
230  grippers_.push_back (gripper);
231  }
232 
235  {
236  return grippers_;
237  }
238 
240  const Grippers_t& grippers () const
241  {
242  return grippers_;
243  }
244 
246  // -----------------------------------------------------------------------
249 
253  BodyPtr_t obstacles () const;
254 
256  size_type nbObjects () const;
257 
260  CollisionObjectPtr_t objectAt (const size_type& i) const;
261 
265  bool collisionTest (const bool stopAtFirstCollision=true);
266 
269  void computeDistances ();
270 
272  const DistanceResults_t& distanceResults () const;
274  // -----------------------------------------------------------------------
278 
280  void numberDeviceData (const size_type& s);
281 
283  size_type numberDeviceData () const;
284 
286  // -----------------------------------------------------------------------
287 
289  virtual std::ostream& print (std::ostream& os) const;
290 
299  fcl::AABB computeAABB() const;
300 
301  void controlComputation (const Computation_t& flag);
302 
303  protected:
305  Device(const std::string& name);
306 
309  void init(const DeviceWkPtr_t& weakPtr);
312  void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
313 
315  Device(const Device& device);
316 
317  private:
318 
320  void resizeState ();
321 
322  protected:
324 
325  DeviceData & d () { return d_; }
326  DeviceData const& d () const { return d_; }
327 
328  inline void invalidate () { d_.invalidate(); }
329 
330  std::string name_;
331  // Grippers
334  // Extra configuration space
336  // Joint linear constraints
337  std::vector<JointLinearConstraint> jointConstraints_;
338  DeviceWkPtr_t weakPtr_;
339 
340  private:
341  Pool<DeviceData> datas_;
342  }; // class Device
343 
344  inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
345  { return device.print(os); }
346 
347  } // namespace pinocchio
348 } // namespace hpp
349 
350 #endif // HPP_PINOCCHIO_DEVICE_HH
hpp::pinocchio::Device::jointConstraints
const std::vector< JointLinearConstraint > & jointConstraints() const
Definition: device.hh:189
hpp::pinocchio::Device::CollisionPair_t
std::pair< JointPtr_t, JointPtr_t > CollisionPair_t
Collision pairs between bodies.
Definition: device.hh:60
hpp::pinocchio::DeviceSync
Definition: device-sync.hh:162
hpp::pinocchio::Grippers_t
std::vector< GripperPtr_t > Grippers_t
Definition: fwd.hh:112
device-sync.hh
hpp::pinocchio::Device
Robot with geometric and dynamic pinocchio.
Definition: device.hh:51
hpp::pinocchio::GeomDataPtr_t
boost::shared_ptr< GeomData > GeomDataPtr_t
Definition: fwd.hh:121
hpp::pinocchio::Pool
Pool of objects.
Definition: pool.hh:48
hpp::pinocchio::value_type
double value_type
Definition: fwd.hh:40
hpp::pinocchio::GripperPtr_t
boost::shared_ptr< Gripper > GripperPtr_t
Definition: fwd.hh:111
hpp::pinocchio::GeomModelPtr_t
boost::shared_ptr< GeomModel > GeomModelPtr_t
Definition: fwd.hh:119
hpp::pinocchio::Device::setGeomData
void setGeomData(GeomDataPtr_t geomDataPtr)
Set Pinocchio geomData corresponding to model.
Definition: device.hh:111
hpp::pinocchio::LiegroupSpacePtr_t
boost::shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:134
hpp::pinocchio::Device::jointConstraints_
std::vector< JointLinearConstraint > jointConstraints_
Definition: device.hh:337
hpp::pinocchio::Device::setGeomModel
void setGeomModel(GeomModelPtr_t geomModelPtr)
Set pinocchio geom.
Definition: device.hh:103
hpp::pinocchio::DeviceConstPtr_t
boost::shared_ptr< const Device > DeviceConstPtr_t
Definition: fwd.hh:105
hpp::pinocchio::DataPtr_t
boost::shared_ptr< Data > DataPtr_t
Definition: fwd.hh:116
hpp::pinocchio::Device::JointLinearConstraint::offset
value_type offset
Definition: device.hh:128
hpp::pinocchio::Device::invalidate
void invalidate()
Definition: device.hh:328
hpp::pinocchio::Device::grippers
const Grippers_t & grippers() const
Return list of grippers of the Device.
Definition: device.hh:240
hpp::pinocchio::Device::configSpace
const LiegroupSpacePtr_t & configSpace() const
Returns a LiegroupSpace representing the configuration space.
Definition: device.hh:178
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:76
HPP_PINOCCHIO_DLLAPI
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
hpp::pinocchio::ExtraConfigSpace
Definition: extra-config-space.hh:34
hpp::pinocchio::Device::d
DeviceData const & d() const
Definition: device.hh:326
hpp::pinocchio::Device::d_
DeviceData d_
Definition: device.hh:323
hpp::pinocchio::Device::configSpaceRnxSOn_
LiegroupSpacePtr_t configSpaceRnxSOn_
Definition: device.hh:333
hpp::pinocchio::Device::RnxSOnConfigSpace
const LiegroupSpacePtr_t & RnxSOnConfigSpace() const
See Joint::RnxSOnConfigurationSpace.
Definition: device.hh:181
fwd.hh
hpp::pinocchio::Joint
Definition: joint.hh:45
deprecated.hh
hpp::pinocchio::Device::JointLinearConstraint
Definition: device.hh:126
hpp::pinocchio::DeviceData
Definition: device-data.hh:41
hpp::pinocchio::Device::extraConfigSpace_
ExtraConfigSpace extraConfigSpace_
Definition: device.hh:335
hpp::pinocchio::Device::weakPtr_
DeviceWkPtr_t weakPtr_
Definition: device.hh:338
hpp::pinocchio::size_type
matrix_t::Index size_type
Definition: fwd.hh:84
hpp::pinocchio::JointPtr_t
boost::shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:109
frame.hh
device-data.hh
hpp
Utility functions.
Definition: body.hh:30
hpp::pinocchio::DistanceResults_t
std::vector< fcl::DistanceResult > DistanceResults_t
Definition: fwd.hh:106
hpp::pinocchio::Configuration_t
vector_t Configuration_t
Definition: fwd.hh:76
hpp::pinocchio::Device::name_
std::string name_
Definition: device.hh:330
hpp::pinocchio::Device::d
DeviceData & d()
Definition: device.hh:325
hpp::pinocchio::Device::extraConfigSpace
ExtraConfigSpace & extraConfigSpace()
Definition: device.hh:205
hpp::pinocchio::Device::extraConfigSpace
const ExtraConfigSpace & extraConfigSpace() const
Definition: device.hh:213
hpp::pinocchio::Device::grippers_
Grippers_t grippers_
Definition: device.hh:332
hpp::pinocchio::CenterOfMassComputation
Definition: center-of-mass-computation.hh:41
pool.hh
hpp::pinocchio::Device::JointLinearConstraint::reference
JointPtr_t reference
Definition: device.hh:129
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:72
hpp::pinocchio::CollisionObjectPtr_t
boost::shared_ptr< CollisionObject > CollisionObjectPtr_t
Definition: fwd.hh:102
extra-config-space.hh
hpp::pinocchio::Computation_t
Computation_t
Definition: device-data.hh:30
hpp::pinocchio::AbstractDevice
Abstract class representing a Device.
Definition: device-sync.hh:29
hpp::pinocchio::Device::grippers
Grippers_t & grippers()
Return list of grippers of the Device.
Definition: device.hh:234
hpp::pinocchio::ModelPtr_t
boost::shared_ptr< Model > ModelPtr_t
Definition: fwd.hh:114
hpp::pinocchio::Device::CollisionPairs_t
std::list< CollisionPair_t > CollisionPairs_t
Definition: device.hh:61
hpp::pinocchio::Frame
Robot frame.
Definition: frame.hh:31
hpp::pinocchio::Device::setModel
void setModel(ModelPtr_t modelPtr)
Set pinocchio model.
Definition: device.hh:100
hpp::pinocchio::operator<<
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:344
hpp::pinocchio::DevicePtr_t
boost::shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:104
hpp::pinocchio::Device::addGripper
void addGripper(const GripperPtr_t &gripper)
Add a gripper to the Device.
Definition: device.hh:228
hpp::pinocchio::BodyPtr_t
boost::shared_ptr< Body > BodyPtr_t
Definition: fwd.hh:96
hpp::pinocchio::CollisionObject
Definition: collision-object.hh:44
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:106
hpp::pinocchio::DeviceData::invalidate
void invalidate()
Definition: device-data.hh:46
config.hh
hpp::pinocchio::Device::name
const std::string & name() const
Get name of device.
Definition: device.hh:79
pinocchio
Definition: collision-object.hh:31
hpp::pinocchio::Device::setDimensionExtraConfigSpace
virtual void setDimensionExtraConfigSpace(const size_type &dimension)
Set dimension of extra configuration space.
Definition: device.hh:216