This file is indexed.

/usr/include/kido/dynamics/Frame.hpp is in libkido-dev 0.1.0+dfsg-2build9.

This file is owned by root:root, with mode 0o644.

The actual contents of the file can be viewed below.

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
/*
 * Copyright (c) 2014-2015, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Michael X. Grey <mxgrey@gatech.edu>
 *
 * Georgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef KIDO_DYNAMICS_FRAME_HPP_
#define KIDO_DYNAMICS_FRAME_HPP_

#include <set>

#include <Eigen/Geometry>

#include "kido/dynamics/Entity.hpp"
#include "kido/math/MathTypes.hpp"

namespace kido {
namespace dynamics {

/// The Frame class serves as the backbone of KIDO's kinematic tree structure.
///
/// Frame inherits Entity, so it exists within a reference Frame. This class
/// keeps track of both its local (relative) and global (world) transforms,
/// velocities, and accelerations. It also notifies every child Entity when
/// a transform, velocity, or acceleration has changed locally or globally.
///
/// Entity class is inherited by using virtual inheritence to solve the
/// so-called "diamond problem". Because of that, the Entity's constructor will
/// be called directly by the most derived class's constructor.
class Frame : public virtual Entity
{
public:
  friend class Entity;
  friend class WorldFrame;

  Frame(const Frame&) = delete;

  /// Destructor
  virtual ~Frame();

  static Frame* World();

  //--------------------------------------------------------------------------
  // Transform
  //--------------------------------------------------------------------------

  /// Get the transform of this Frame with respect to its parent Frame
  virtual const Eigen::Isometry3d& getRelativeTransform() const = 0;

  /// Get the transform of this Frame with respect to the World Frame
  const Eigen::Isometry3d& getWorldTransform() const;

  /// Get the transform of this Frame with respect to some other Frame
  Eigen::Isometry3d getTransform(
      const Frame* _withRespectTo = Frame::World()) const;

  /// Get the transform of this Frame with respect to some other Frame. It can
  /// be expressed in the coordinates of any Frame.
  Eigen::Isometry3d getTransform(const Frame* withRespectTo,
                                 const Frame* inCoordinatesOf) const;

  //-------------------------------------------------------------------------
  // Velocity
  //-------------------------------------------------------------------------

  /// Get the spatial velocity of this Frame relative to its parent Frame, in
  /// its own coordinates.
  virtual const Eigen::Vector6d& getRelativeSpatialVelocity() const = 0;

  /// Get the total spatial velocity of this Frame in the coordinates of this
  /// Frame.
  const Eigen::Vector6d& getSpatialVelocity() const;

  /// Get the spatial velocity of this Frame relative to some other Frame. It
  /// can be expressed in the coordinates of any Frame.
  Eigen::Vector6d getSpatialVelocity(const Frame* _relativeTo,
                                     const Frame* _inCoordinatesOf) const;

  /// Get the spatial velocity of a fixed point in this Frame. The velocity is
  /// in coordinates of this Frame and is relative to the World Frame.
  Eigen::Vector6d getSpatialVelocity(const Eigen::Vector3d& _offset) const;

  /// Get the spatial velocity of a fixed point in this Frame.
  Eigen::Vector6d getSpatialVelocity(const Eigen::Vector3d& _offset,
                                     const Frame* _relativeTo,
                                     const Frame* _inCoordinatesOf) const;

  /// Get the linear portion of classical velocity of this Frame relative to
  /// some other Frame. It can be expressed in the coordinates of any Frame.
  Eigen::Vector3d getLinearVelocity(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const;

  /// Get the linear velocity of a point that is fixed in this Frame. You can
  /// specify a relative Frame, and it can be expressed in the coordinates of
  /// any Frame.
  Eigen::Vector3d getLinearVelocity(
      const Eigen::Vector3d& _offset,
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const;

  /// Get the angular portion of classical velocity of this Frame relative to
  /// some other Frame. It can be expressed in the coordinates of any Frame.
  Eigen::Vector3d getAngularVelocity(
      const Frame* _relativeTo = Frame::World(),
      const Frame* _inCoordinatesOf = Frame::World()) const;

  //--------------------------------------------------------------------------
  // Acceleration
  //--------------------------------------------------------------------------

  /// Get the spatial acceleration of this Frame relative to its parent Frame,
  /// in the coordinates of this Frame.
  virtual const Eigen::Vector6d& getRelativeSpatialAcceleration() const = 0;

  /// The Featherstone ABI algorithm exploits a component of the spatial
  /// acceleration which we refer to as the partial acceleration, accessible
  /// by getPartialAcceleration(). We save operations during our forward
  /// kinematics by computing and storing the partial acceleration separately
  /// from the rest of the Frame's acceleration. getPrimaryRelativeAcceleration()
  /// will return the portion of the relative spatial acceleration that is not
  /// contained in the partial acceleration. To get the full spatial
  /// acceleration of this Frame relative to its parent Frame, use
  /// getRelativeSpatialAcceleration(). To get the full spatial acceleration
  /// of this Frame relative to the World Frame, use getSpatialAcceleration().
  virtual const Eigen::Vector6d& getPrimaryRelativeAcceleration() const = 0;

  /// The Featherstone ABI algorithm exploits a component of the spatial
  /// acceleration which we refer to as the partial acceleration. This function
  /// returns that component of acceleration.
  virtual const Eigen::Vector6d& getPartialAcceleration() const = 0;

  /// Get the total spatial acceleration of this Frame in the coordinates of
  /// this Frame.
  const Eigen::Vector6d& getSpatialAcceleration() const;

  /// Get the spatial acceleration of this Frame relative to some other Frame.
  /// It can be expressed in the coordinates of any Frame.
  Eigen::Vector6d getSpatialAcceleration(const Frame* _relativeTo,
                                         const Frame* _inCoordinatesOf) const;

  /// Get the spatial acceleration of a fixed point in this Frame. The
  /// acceleration is in coordinates of this Frame and is relative to the World
  /// Frame.
  Eigen::Vector6d getSpatialAcceleration(const Eigen::Vector3d& _offset) const;

  /// Get the spatial acceleration of a fixed point in this Frame
  Eigen::Vector6d getSpatialAcceleration(const Eigen::Vector3d& _offset,
                                         const Frame* _relativeTo,
                                         const Frame* _inCoordinatesOf) const;

  /// Get the linear portion of classical acceleration of this Frame relative to
  /// some other Frame. It can be expressed in the coordinates of any Frame.
  Eigen::Vector3d getLinearAcceleration(
      const Frame* _relativeTo=Frame::World(),
      const Frame* _inCoordinatesOf=Frame::World()) const;

  Eigen::Vector3d getLinearAcceleration(
      const Eigen::Vector3d& _offset,
      const Frame* _relativeTo=Frame::World(),
      const Frame* _inCoordinatesOf=Frame::World()) const;

  /// Get the angular portion of classical acceleration of this Frame relative
  /// to some other Frame. It can be expressed in the coordinates of any Frame.
  Eigen::Vector3d getAngularAcceleration(
      const Frame* _relativeTo=Frame::World(),
      const Frame* _inCoordinatesOf=Frame::World()) const;

  //--------------------------------------------------------------------------
  // Relationships
  //--------------------------------------------------------------------------

  /// Get a container with the Entities that are children of this Frame.
  /// std::set is used because Entities may be arbitrarily added and removed
  /// from a parent Frame, and each entry should be unique. std::set makes this
  /// procedure easier.
  const std::set<Entity*>& getChildEntities();

  /// Get a container with the Entities that are children of this Frame. Note
  /// that this is version is slightly less efficient than the non-const version
  /// because it needs to rebuild a set where each pointer is converted to be a
  /// const pointer.
  const std::set<const Entity*> getChildEntities() const;

  /// Get the number of Entities that are currently children of this Frame.
  size_t getNumChildEntities() const;

  /// Get a container with the Frames that are children of this Frame.
  /// std::set is used because Frames may be arbitrarily added and removed
  /// from a parent Frame, and each entry should be unique.
  const std::set<Frame*>& getChildFrames();

  /// Get a container with the Frames that are children of this Frame. Note
  /// that this version is less efficient than the non-const version because
  /// it needs to rebuild a set so that the entries are const.
  std::set<const Frame*> getChildFrames() const;

  /// Get the number of Frames that are currently children of this Frame.
  size_t getNumChildFrames() const;

  /// Returns true if this Frame is the World Frame
  bool isWorld() const;

  //--------------------------------------------------------------------------
  // Rendering
  //--------------------------------------------------------------------------

  // Render this Frame as well as any Entities it contains
  virtual void draw(
      renderer::RenderInterface *_ri = nullptr,
      const Eigen::Vector4d &_color = Eigen::Vector4d::Ones(),
      bool _useDefaultColor = true, int _depth = 0) const override;

  /// Notify this Frame and all its children that its pose has changed
  virtual void notifyTransformUpdate() override;

  /// Notify this Frame and all its children that its velocity has changed
  virtual void notifyVelocityUpdate() override;

  /// Notify this Frame and all its children that its acceleration has changed
  virtual void notifyAccelerationUpdate() override;

protected:

  /// Used when constructing a pure abstract class, because calling the Frame
  /// constructor is just a formality
  enum ConstructAbstract_t { ConstructAbstract };

  /// Constructor for typical usage
  explicit Frame(Frame* _refFrame, const std::string& _name);

  /// Constructor for use by pure abstract classes
  explicit Frame(ConstructAbstract_t);

  // Documentation inherited
  virtual void changeParentFrame(Frame* _newParentFrame) override;

  /// Called during a parent Frame change to allow extensions of the Frame class
  /// to handle new children in customized ways. This function is a no op unless
  /// an inheriting class (such as BodyNode) overrides it.
  virtual void processNewEntity(Entity* _newChildEntity);

  /// Called when a child Entity is removed from its parent Frame. This allows
  /// special post-processing to be performed for extensions of the Frame class.
  virtual void processRemovedEntity(Entity* _oldChildEntity);

private:

  /// Used when constructing the World
  enum ConstructWorld_t { ConstructWorld };

  /// Constructor only to be used by the WorldFrame class
  explicit Frame(ConstructWorld_t);

protected:
  /// World transform of this Frame. This object is mutable to enable
  /// auto-updating to happen in the const member getWorldTransform() function
  ///
  /// Do not use directly! Use getWorldTransform() to access this quantity
  mutable Eigen::Isometry3d mWorldTransform;

  /// Total velocity of this Frame, in the coordinates of this Frame
  ///
  /// Do not use directly! Use getSpatialVelocity() to access this quantity
  mutable Eigen::Vector6d mVelocity;

  /// Total acceleration of this Frame, in the coordinates of this Frame
  ///
  /// Do not use directly! Use getSpatialAcceleration() to access this quantity
  mutable Eigen::Vector6d mAcceleration;

  /// Container of this Frame's child Frames.
  std::set<Frame*> mChildFrames;

  /// Container of this Frame's child Entities.
  std::set<Entity*> mChildEntities;

private:
  /// Contains whether or not this is the World Frame
  const bool mAmWorld;

public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/// The WorldFrame class is a class that is used internally to create the
/// singleton World Frame. This class cannot be instantiated directly: you must
/// use the Frame::World() function to access it. Only one World Frame exists
/// in any application.
class WorldFrame : public Frame
{
public:
  friend class Frame;

  /// Always returns the Identity Transform
  const Eigen::Isometry3d& getRelativeTransform() const override;

  /// Always returns a zero vector
  const Eigen::Vector6d& getRelativeSpatialVelocity() const override;

  /// Always returns a zero vector
  const Eigen::Vector6d& getRelativeSpatialAcceleration() const override;

  /// Always returns a zero vector
  const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override;

  /// Always returns a zero vector
  const Eigen::Vector6d& getPartialAcceleration() const override;

private:
  /// This may only be constructed by the Frame class
  explicit WorldFrame();

private:
  /// This is set to Identity and never changes
  const Eigen::Isometry3d mRelativeTf;

  /// This is set to a Zero vector and never changes
  static const Eigen::Vector6d mZero;

public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace dynamics
} // namespace kido

#endif // KIDO_DYNAMICS_FRAME_HPP_