hpp-pinocchio  4.12.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
collision-object.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_COLLISION_OBJECT_HH
21 # define HPP_PINOCCHIO_COLLISION_OBJECT_HH
22 
23 #include <map>
24 
25 # include <pinocchio/multibody/fwd.hpp>
26 
28 # include <hpp/pinocchio/config.hh>
29 # include <hpp/pinocchio/fwd.hh>
30 
31 namespace pinocchio
32 {
33  struct GeometryObject;
34 }
35 
36 namespace hpp {
37  namespace pinocchio {
39 
45  public:
46 
47  typedef std::vector<GeomIndex> GeomIndexList;
48  typedef std::map < JointIndex, GeomIndexList > ObjectVec_t;
49 
52  const GeomIndex geom );
53 
57  GeomDataPtr_t geomData,
58  const GeomIndex geom );
59 
60  const std::string& name () const;
61 
63  const GeometryObject & pinocchio () const;
64  GeometryObject & pinocchio ();
65 
67  CollisionGeometryPtr_t geometry () const;
68 
70  FclConstCollisionObjectPtr_t fcl (const GeomData& data) const;
71  FclCollisionObjectPtr_t fcl ( GeomData& data) const;
72  FclConstCollisionObjectPtr_t fcl () const ;
74 
78  FclCollisionObjectPtr_t fcl (DeviceData& d) const;
79  FclConstCollisionObjectPtr_t fcl (const DeviceData& d) const;
80 
82  const JointIndex& jointIndex () const { return jointIndex_; }
83 
85  JointPtr_t joint () ;
86  JointConstPtr_t joint () const;
87 
89  const Transform3f& positionInJointFrame () const;
90 
96  fcl::Transform3f getFclTransform () const;
97  const Transform3f& getTransform () const;
98  const Transform3f& getTransform (const DeviceData& d) const;
99 
100  const GeomIndex& indexInModel () const
101  {
102  return geomInModelIndex;
103  }
104 
109  void move (const Transform3f& position);
110 
111  protected:
112 
114  void selfAssert() const;
115 
116  private:
117  GeomData& geomData() const;
118  GeomData& geomData(DeviceData& d) const;
119  const GeomData& geomData(const DeviceData& d) const;
120 
121  DevicePtr_t devicePtr;
122  GeomModelPtr_t geomModel_;
123  // If geomData_ is not null when the object is part of the environment.
124  // Otherwise, it is null.
125  GeomDataPtr_t geomData_;
126  JointIndex jointIndex_;
127  GeomIndex geomInModelIndex; // Index in global model list.
128  }; // class CollisionObject
129  } // namespace pinocchio
130 } // namespace hpp
131 
132 #endif // HPP_PINOCCHIO_COLLISION_OBJECT_HH
shared_ptr< GeomModel > GeomModelPtr_t
Definition: fwd.hh:121
shared_ptr< const Joint > JointConstPtr_t
Definition: fwd.hh:112
std::vector< GeomIndex > GeomIndexList
Definition: collision-object.hh:47
const GeomIndex & indexInModel() const
Definition: collision-object.hh:100
Utility functions.
Definition: body.hh:30
::pinocchio::GeomIndex GeomIndex
Definition: fwd.hh:64
Definition: device-data.hh:41
const JointIndex & jointIndex() const
Get joint index.
Definition: collision-object.hh:82
fcl::CollisionObject * FclCollisionObjectPtr_t
Definition: fwd.hh:102
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
shared_ptr< GeomData > GeomDataPtr_t
Definition: fwd.hh:123
std::map< JointIndex, GeomIndexList > ObjectVec_t
Definition: collision-object.hh:48
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
::pinocchio::GeometryData GeomData
Definition: fwd.hh:68
const fcl::CollisionObject * FclConstCollisionObjectPtr_t
Definition: fwd.hh:103
Definition: collision-object.hh:44
shared_ptr< Joint > JointPtr_t
Definition: fwd.hh:111
::pinocchio::GeometryObject GeometryObject
Definition: collision-object.hh:38
Definition: collision-object.hh:31
::pinocchio::SE3 Transform3f
Definition: fwd.hh:69
::pinocchio::JointIndex JointIndex
Definition: fwd.hh:62