This file is indexed.

/usr/include/kido/dynamics/FreeJoint.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
/*
 * Copyright (c) 2013-2015, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Jeongseok Lee <jslee02@gmail.com>
 *
 * 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_FREEJOINT_HPP_
#define KIDO_DYNAMICS_FREEJOINT_HPP_

#include <string>

#include <Eigen/Dense>

#include "kido/dynamics/MultiDofJoint.hpp"

namespace kido {
namespace dynamics {

/// class FreeJoint
class FreeJoint : public MultiDofJoint<6>
{
public:

  friend class Skeleton;

  struct Properties : MultiDofJoint<6>::Properties
  {
    Properties(const MultiDofJoint<6>::Properties& _properties =
                                                MultiDofJoint<6>::Properties());

    virtual ~Properties() = default;
  };

  FreeJoint(const FreeJoint&) = delete;

  /// Destructor
  virtual ~FreeJoint();

  /// Get the Properties of this FreeJoint
  Properties getFreeJointProperties() const;

  // Documentation inherited
  virtual const std::string& getType() const override;

  /// Get joint type for this class
  static const std::string& getStaticType();

  // Documentation inherited
  virtual bool isCyclic(size_t _index) const override;

  /// Convert a transform into a 6D vector that can be used to set the positions
  /// of a FreeJoint. The positions returned by this function will result in a
  /// relative transform of
  /// getTransformFromParentBodyNode() * _tf * getTransformFromChildBodyNode().inverse()
  /// between the parent BodyNode and the child BodyNode frames when applied to
  /// a FreeJoint.
  static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf);

  /// Convert a FreeJoint-style 6D vector into a transform
  static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);

  /// If the given joint is a FreeJoint, then set the transform of the given
  /// Joint's child BodyNode so that its transform with respect to
  /// "withRespecTo" is equal to "tf".
  static void setTransform(Joint* joint,
                           const Eigen::Isometry3d& tf,
                           const Frame* withRespectTo = Frame::World());

  /// If the parent Joint of the given BodyNode is a FreeJoint, then set the
  /// transform of the given BodyNode so that its transform with respect to
  /// "withRespecTo" is equal to "tf".
  static void setTransform(BodyNode* bodyNode,
                           const Eigen::Isometry3d& tf,
                           const Frame* withRespectTo = Frame::World());

  /// Apply setTransform(bodyNode, tf, withRespecTo) for all the root BodyNodes
  /// of the given Skeleton. If false is passed in "applyToAllRootBodies", then
  /// it will be applied to only the default root BodyNode that will be obtained
  /// by Skeleton::getRootBodyNode().
  static void setTransform(Skeleton* skeleton,
                           const Eigen::Isometry3d& tf,
                           const Frame* withRespectTo = Frame::World(),
                           bool applyToAllRootBodies = true);

  /// Set the transform, spatial velocity, and spatial acceleration of the child
  /// BodyNode relative to an arbitrary Frame. The reference frame can be
  /// arbitrarily specified.
  ///
  /// If you want to set more than one kind of Cartetian coordinates (e.g.,
  /// transform and spatial velocity) at the same time, you should call
  /// corresponding setters in a certain order (transform -> velocity ->
  /// acceleration), If you don't velocity or acceleration can be corrupted by
  /// transform or velocity. This function calls the corresponding setters in
  /// the right order so that all the desired Cartetian coordinates are properly
  /// set.
  ///
  /// Pass nullptr for "newTransform", "newSpatialVelocity", or
  /// "newSpatialAcceleration" if you don't want to set them.
  ///
  /// \param[in] newTransform Desired transform of the child BodyNode.
  /// \param[in] withRespectTo The relative Frame of "newTransform".
  /// \param[in] newSpatialVelocity Desired spatial velocity of the child
  /// BodyNode.
  /// \param[in] velrelativeTo The relative frame of "newSpatialVelocity".
  /// \param[in] velinCoordinatesOf The reference frame of "newSpatialVelocity".
  /// \param[in] newSpatialAcceleration Desired spatial acceleration of the
  /// child BodyNode.
  /// \param[in] accrelativeTo The relative frame of "newSpatialAcceleration".
  /// \param[in] accinCoordinatesOf The reference frame of
  /// "newSpatialAcceleration".
  void setSpatialMotion(
      const Eigen::Isometry3d* newTransform,
      const Frame* withRespectTo,
      const Eigen::Vector6d* newSpatialVelocity,
      const Frame* velRelativeTo,
      const Frame* velInCoordinatesOf,
      const Eigen::Vector6d* newSpatialAcceleration,
      const Frame* accRelativeTo,
      const Frame* accInCoordinatesOf);

  /// Set the transform of the child BodyNode relative to the parent BodyNode
  /// \param[in] newTransform Desired transform of the child BodyNode.
  void setRelativeTransform(const Eigen::Isometry3d& newTransform);

  /// Set the transform of the child BodyNode relative to an arbitrary Frame.
  /// \param[in] newTransform Desired transform of the child BodyNode.
  /// \param[in] withRespectTo The relative Frame of "newTransform".
  void setTransform(const Eigen::Isometry3d& newTransform,
                    const Frame* withRespectTo = Frame::World());

  /// Set the spatial velocity of the child BodyNode relative to the parent
  /// BodyNode.
  /// \param[in] newSpatialVelocity Desired spatial velocity of the child
  /// BodyNode. The reference frame of "newSpatialVelocity" is the child
  /// BodyNode.
  void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity);

  /// Set the spatial velocity of the child BodyNode relative to the parent
  /// BodyNode.
  /// \param[in] newSpatialVelocity Desired spatial velocity of the child
  /// BodyNode.
  /// \param[in] inCoordinatesOf The reference frame of "newSpatialVelocity".
  void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity,
                                  const Frame* inCoordinatesOf);

  /// Set the spatial velocity of the child BodyNode relative to an arbitrary
  /// Frame.
  /// \param[in] newSpatialVelocity Desired spatial velocity of the child
  /// BodyNode.
  /// \param[in] relativeTo The relative frame of "newSpatialVelocity".
  /// \param[in] inCoordinatesOf The reference frame of "newSpatialVelocity".
  void setSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity,
                          const Frame* relativeTo,
                          const Frame* inCoordinatesOf);

  /// Set the linear portion of classical velocity of the child BodyNode
  /// relative to an arbitrary Frame.
  ///
  /// Note that the angular portion of claasical velocity of the child
  /// BodyNode will not be changed after this function called.
  ///
  /// \param[in] relativeTo The relative frame of "newLinearVelocity".
  /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity".
  void setLinearVelocity(const Eigen::Vector3d& newLinearVelocity,
                         const Frame* relativeTo = Frame::World(),
                         const Frame* inCoordinatesOf = Frame::World());

  /// Set the angular portion of classical velocity of the child BodyNode
  /// relative to an arbitrary Frame.
  ///
  /// Note that the linear portion of claasical velocity of the child
  /// BodyNode will not be changed after this function called.
  ///
  /// \param[in] relativeTo The relative frame of "newLinearVelocity".
  /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity".
  void setAngularVelocity(const Eigen::Vector3d& newAngularVelocity,
                          const Frame* relativeTo = Frame::World(),
                          const Frame* inCoordinatesOf = Frame::World());

  /// Set the spatial acceleration of the child BodyNode relative to the parent
  /// BodyNode.
  /// \param[in] newSpatialVelocity Desired spatial acceleration of the child
  /// BodyNode. The reference frame of "newSpatialAcceleration" is the child
  /// BodyNode.
  void setRelativeSpatialAcceleration(
      const Eigen::Vector6d& newSpatialAcceleration);

  /// Set the spatial acceleration of the child BodyNode relative to the parent
  /// BodyNode.
  /// \param[in] newSpatialAcceleration Desired spatial acceleration of the
  /// child BodyNode.
  /// \param[in] inCoordinatesOf The reference frame of
  /// "newSpatialAcceleration".
  void setRelativeSpatialAcceleration(
      const Eigen::Vector6d& newSpatialAcceleration,
      const Frame* inCoordinatesOf);

  /// Set the spatial acceleration of the child BodyNode relative to an
  /// arbitrary Frame.
  /// \param[in] newSpatialAcceleration Desired spatial acceleration of the
  /// child BodyNode.
  /// \param[in] relativeTo The relative frame of "newSpatialAcceleration".
  /// \param[in] inCoordinatesOf The reference frame of
  /// "newSpatialAcceleration".
  void setSpatialAcceleration(const Eigen::Vector6d& newSpatialAcceleration,
                              const Frame* relativeTo,
                              const Frame* inCoordinatesOf);

  /// Set the linear portion of classical acceleration of the child BodyNode
  /// relative to an arbitrary Frame.
  ///
  /// Note that the angular portion of claasical acceleration of the child
  /// BodyNode will not be changed after this function called.
  ///
  /// \param[in] relativeTo The relative frame of "newLinearVelocity".
  /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity".
  void setLinearAcceleration(const Eigen::Vector3d& newLinearAcceleration,
                             const Frame* relativeTo = Frame::World(),
                             const Frame* inCoordinatesOf = Frame::World());

  /// Set the angular portion of classical acceleration of the child BodyNode
  /// relative to an arbitrary Frame.
  ///
  /// Note that the linear portion of claasical acceleration of the child
  /// BodyNode will not be changed after this function called.
  ///
  /// \param[in] relativeTo The relative frame of "newLinearVelocity".
  /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity".
  void setAngularAcceleration(const Eigen::Vector3d& newAngularAcceleration,
                              const Frame* relativeTo = Frame::World(),
                              const Frame* inCoordinatesOf = Frame::World());

  // Documentation inherited
  Eigen::Matrix6d getLocalJacobianStatic(
      const Eigen::Vector6d& _positions) const override;

  // Documentation inherited
  Eigen::Vector6d getPositionDifferencesStatic(
      const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const override;

protected:

  /// Constructor called by Skeleton class
  FreeJoint(const Properties& _properties);

  // Documentation inherited
  Joint* clone() const override;

  using MultiDofJoint::getLocalJacobianStatic;

  // Documentation inherited
  void integratePositions(double _dt) override;

  // Documentation inherited
  void updateDegreeOfFreedomNames() override;

  // Documentation inherited
  void updateLocalTransform() const override;

  // Documentation inherited
  void updateLocalJacobian(bool _mandatory = true) const override;

  // Documentation inherited
  void updateLocalJacobianTimeDeriv() const override;

protected:

  /// Access mQ, which is an auto-updating variable
  const Eigen::Isometry3d& getQ() const;

  /// Transformation matrix dependent on generalized coordinates
  ///
  /// Do not use directly! Use getQ() to access this
  mutable Eigen::Isometry3d mQ;

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

}  // namespace dynamics
}  // namespace kido

#endif  // KIDO_DYNAMICS_FREEJOINT_HPP_