geometry.hpp 9.44 KB
Newer Older
1
//
2
// Copyright (c) 2015-2016 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_geom_hpp__
#define __se3_geom_hpp__


22
#include "pinocchio/multibody/fcl.hpp"
23
#include "pinocchio/multibody/model.hpp"
24
#include "pinocchio/container/aligned-vector.hpp"
25

26
27
#include <iostream>

28
#include <boost/foreach.hpp>
29
30
#include <map>
#include <list>
31
#include <utility>
32
#include <assert.h>
33
34
35
36


namespace se3
{
37
  
38
  struct GeometryModel
39
  {
40
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
jcarpent's avatar
jcarpent committed
41
    
42
43
    /// \brief The number of GeometryObjects
    Index ngeoms;
44
45

    /// \brief Vector of GeometryObjects used for collision computations
46
    container::aligned_vector<GeometryObject> geometryObjects;
47
48
49
    ///
    /// \brief Vector of collision pairs.
    ///
50
    std::vector<CollisionPair> collisionPairs;
51
  
52
53
    GeometryModel()
      : ngeoms(0)
54
      , geometryObjects()
55
56
57
58
59
      , collisionPairs()
    { 
      const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
      collisionPairs.reserve(num_max_collision_pairs);
    }
60
61
62

    ~GeometryModel() {};

63
64
65
66
    /**
     * @brief      Add a geometry object to a GeometryModel
     *
     * @param[in]  object     Object 
67
     * @param[in]  model      Corresponding model, used to assert the attributes of object.
68
     * @param[in]  autofillJointParent if true, set jointParent from frameParent.
69
70
     *
     * @return     The index of the new added GeometryObject in geometryObjects
71
     * @note object is a nonconst copy to ease the insertion code.
72
     */
73
    inline GeomIndex addGeometryObject(GeometryObject object,
74
75
                                       const Model & model,
                                       const bool autofillJointParent = false);
76
77

    /**
78
     * @brief      Return the index of a GeometryObject given by its name.
79
80
81
82
83
     *
     * @param[in]  name  Name of the GeometryObject
     *
     * @return     Index of the corresponding GeometryObject
     */
84
85
    GeomIndex getGeometryId(const std::string & name) const;

86
87
    
    /**
88
     * @brief      Check if a GeometryObject  given by its name exists.
89
90
91
     *
     * @param[in]  name  Name of the GeometryObject
     *
92
     * @return     True if the GeometryObject exists in the geometryObjects.
93
     */
94
95
    bool existGeometryName(const std::string & name) const;

96
97

    /**
98
     * @brief      Get the name of a GeometryObject given by its index.
99
100
101
102
103
     *
     * @param[in]  index  Index of the GeometryObject
     *
     * @return     Name of the GeometryObject
     */
104
    PINOCCHIO_DEPRECATED const std::string & getGeometryName(const GeomIndex index) const;
105

106
#ifdef WITH_HPP_FCL
107
108
109
110
111
112
113
114
115
116
117
    ///
    /// \brief Add a collision pair into the vector of collision_pairs.
    ///        The method check before if the given CollisionPair is already included.
    ///
    /// \param[in] pair The CollisionPair to add.
    ///
    void addCollisionPair (const CollisionPair & pair);
    
    ///
    /// \brief Add all possible collision pairs.
    ///
118
119
120
    /// \note Collision pairs between geometries of having the same parent joint
    ///       are not added.
    ///
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
    void addAllCollisionPairs();
   
    ///
    /// \brief Remove if exists the CollisionPair from the vector collision_pairs.
    ///
    /// \param[in] pair The CollisionPair to remove.
    ///
    void removeCollisionPair (const CollisionPair& pair);
    
    ///
    /// \brief Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
    void removeAllCollisionPairs ();
   
    ///
    /// \brief Check if a collision pair exists in collisionPairs.
    ///        See also findCollisitionPair(const CollisionPair & pair).
    ///
    /// \param[in] pair The CollisionPair.
    ///
    /// \return True if the CollisionPair exists, false otherwise.
    ///
    bool existCollisionPair (const CollisionPair & pair) const;
    
    ///
    /// \brief Return the index of a given collision pair in collisionPairs.
    ///
    /// \param[in] pair The CollisionPair.
    ///
    /// \return The index of the CollisionPair in collisionPairs.
    ///
151
    PairIndex findCollisionPair (const CollisionPair & pair) const;
152
    
153
#endif // WITH_HPP_FCL
154

jcarpent's avatar
jcarpent committed
155
156
    friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
  }; // struct GeometryModel
157

158
  struct GeometryData
159
  {
160
161
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
162
    ///
163
    /// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
164
    ///        See updateGeometryPlacements to update the placements.
165
    ///
166
167
168
    /// oMg is used for pinocchio (kinematics) computation but is translated to fcl type
    /// for fcl (collision) computation. The copy is done in collisionObjects[i]->setTransform(.)
    ///
169
    container::aligned_vector<se3::SE3> oMg;
170

171
#ifdef WITH_HPP_FCL
172
    ///
173
    /// \brief Collision objects (ie a fcl placed geometry).
174
    ///
175
176
177
    /// The object contains a pointer on the collision geometries contained in geomModel.geometryObjects.
    /// \sa GeometryModel::geometryObjects and GeometryObjects
    ///
178
    std::vector<fcl::CollisionObject> collisionObjects;
179

180
181
182
    ///
    /// \brief Vector of collision pairs.
    ///
183
    std::vector<bool> activeCollisionPairs;
184

185
186
187
188
189
    ///
    /// \brief Defines what information should be computed by distance computation.
    ///
    fcl::DistanceRequest distanceRequest;

190
    ///
191
    /// \brief Vector gathering the result of the distance computation for all the collision pairs.
192
    ///
193
    std::vector <fcl::DistanceResult> distanceResults;
194
    
195
196
197
198
199
    ///
    /// \brief Defines what information should be computed by collision test.
    ///
    fcl::CollisionRequest collisionRequest;

200
    ///
201
    /// \brief Vector gathering the result of the collision computation for all the collision pairs.
202
    ///
203
    std::vector <fcl::CollisionResult> collisionResults;
204

205
206
207
208
209
    ///
    /// \brief Radius of the bodies, i.e. distance of the further point of the geometry model
    /// attached to the body from the joint center.
    ///
    std::vector<double> radius;
210
211
212
213
214

    ///
    /// \brief index of the collision pair
    ///
    /// It is used by some method to return additional information. For instance,
215
    /// the algo computeCollisions() sets it to the first colliding pair.
216
    ///
217
    PairIndex collisionPairIndex;
218

219
220
221
222
223
224
225
226
227
228
229
230
231
    typedef std::vector<GeomIndex> GeomIndexList;

    /// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
    /// 
    /// The map lists the collision GeometryObjects associated to a given joint Id.
    ///  Inner objects can be seen as geometry objects that directly move when the associated joint moves
    std::map < JointIndex, GeomIndexList >  innerObjects;

    /// \brief A list of associated collision GeometryObjects to a given joint Id
    ///
    /// Outer objects can be seen as geometry objects that may often be
    /// obstacles to the Inner objects of given joint
    std::map < JointIndex, GeomIndexList >  outerObjects;
232
#endif // WITH_HPP_FCL   
233

234
    GeometryData(const GeometryModel & geomModel);
235
    ~GeometryData() {};
236

237
#ifdef WITH_HPP_FCL
238

239
240
241
242
243
    /// Fill both innerObjects and outerObjects maps, from vectors collisionObjects and 
    /// collisionPairs. 
    ///
    /// This simply corresponds to storing in a re-arranged manner the information stored
    /// in geomModel.geometryObjects and geomModel.collisionPairs.
Guilhem Saurel's avatar
Guilhem Saurel committed
244
    /// \param[in] geomModel the geometry model (const)
245
246
247
    ///
    /// \warning Outer objects are not duplicated (i.e. if a is in outerObjects[b], then
    /// b is not in outerObjects[a]).
248
    void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
249

250
251
252
253
254
255
256
257
    /// Activate a collision pair, for which collisions and distances would now be computed.
    ///
    /// A collision (resp distance) between to geometries of GeomModel::geometryObjects
    /// is computed *iff* the corresponding pair has been added in GeomModel::collisionPairs *AND*
    /// it is active, i.e. the corresponding boolean in GeomData::activePairs is true. The second
    /// condition can be used to temporarily remove a pair without touching the model, in a versatile
    /// manner. 
    /// \param[in] pairId the index of the pair in GeomModel::collisionPairs vector.
Guilhem Saurel's avatar
Guilhem Saurel committed
258
    /// \param[in] flag value of the activation boolean (true by default).
259
    void activateCollisionPair(const PairIndex pairId,const bool flag=true);
260
261
262
263
264

    /// Deactivate a collision pair.
    ///
    /// Calls indeed GeomData::activateCollisionPair(pairId,false)
    /// \sa activateCollisionPair
265
    void deactivateCollisionPair(const PairIndex pairId);
266

267
#endif //WITH_HPP_FCL
268
    friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
269
    
jcarpent's avatar
jcarpent committed
270
  }; // struct GeometryData
271
272
273

} // namespace se3

274
275
276
277
278
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/geometry.hxx"

279
#endif // ifndef __se3_geom_hpp__