Kindr
Kinematics and Dynamics for Robotics
RotationVector.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include <cmath>
32 
33 #include <Eigen/Geometry>
34 
35 #include "kindr/common/common.hpp"
38 
39 namespace kindr {
40 
41 
57 template<typename PrimType_>
58 class RotationVector : public RotationBase<RotationVector<PrimType_>> {
59  private:
62  typedef Eigen::Matrix<PrimType_, 3, 1> Base;
63 
66  Base vector_;
67 
68  public:
72  typedef Base Implementation;
76  typedef PrimType_ Scalar;
77 
80  typedef Base Vector;
81 
85  : vector_(Base::Zero()) {
86  }
87 
94  RotationVector(Scalar x, Scalar y, Scalar z)
95  : vector_(x,y,z) {
96  }
97 
98 
103  explicit RotationVector(const Base& other) // explicit on purpose
104  : vector_(other) {
105  }
106 
110  template<typename OtherDerived_>
111  inline explicit RotationVector(const RotationBase<OtherDerived_>& other)
112  : vector_(internal::ConversionTraits<RotationVector, OtherDerived_>::convert(other.derived()).toImplementation()) {
113  }
114 
119  template<typename OtherDerived_>
121  this->toImplementation() = internal::ConversionTraits<RotationVector, OtherDerived_>::convert(other.derived()).toImplementation();
122  return *this;
123  }
124 
129  template<typename OtherDerived_>
131  this->toImplementation() = internal::ConversionTraits<RotationVector, OtherDerived_>::convert(other.derived()).toImplementation();
132  return *this;
133  }
134 
139  return RotationVector(-this->toImplementation());
140  }
141 
146  *this = inverted();
147  return *this;
148  }
149 
153  inline Implementation& toImplementation() {
154  return static_cast<Implementation&>(vector_);
155  }
156 
160  inline const Implementation& toImplementation() const {
161  return static_cast<const Implementation&>(vector_);
162  }
163 
168  vector_ = Base::Zero();
169  return *this;
170  }
171 
175  inline const Vector vector() const {
176  return this->toImplementation();
177  }
178 
184  inline void setVector(Scalar x, Scalar y, Scalar z) {
185  vector_ << x, y, z;
186  }
187 
190  inline void setVector(const Implementation& vector) {
191  vector_ = vector;
192  }
193 
197  inline Scalar x() const {
198  return vector_(0);
199  }
200 
204  inline Scalar y() const {
205  return vector_(1);
206  }
207 
211  inline Scalar z() const {
212  return vector_(2);
213  }
214 
217  inline void setX(Scalar x) {
218  vector_(0) = x;
219  }
220 
223  inline void setY(Scalar y) {
224  vector_(1) = y;
225  }
226 
229  inline void setZ(Scalar z) {
230  vector_(2) = z;
231  }
232 
238  AngleAxis<PrimType_> angleAxis(*this);
239  RotationVector rotationVector(angleAxis.getUnique());
240  return rotationVector;
241  }
242 
247  *this = getUnique();
248  return *this;
249  }
250 
255  using RotationBase<RotationVector<PrimType_>>::operator*;
256 
260  friend std::ostream& operator << (std::ostream& out, const RotationVector& rotationVector) {
261  out << rotationVector.toImplementation().transpose();
262  return out;
263  }
264 };
265 
274 
275 
276 
277 namespace internal {
278 
279 template<typename PrimType_>
280 class get_scalar<RotationVector<PrimType_>> {
281  public:
282  typedef PrimType_ Scalar;
283 };
284 
285 template<typename PrimType_>
286 class get_matrix3X<RotationVector<PrimType_>>{
287  public:
288  typedef int IndexType;
289 
290  template <IndexType Cols>
291  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
292 };
293 
294 
295 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
296  * Conversion Traits
297  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
298 template<typename DestPrimType_, typename SourcePrimType_>
299 class ConversionTraits<RotationVector<DestPrimType_>, RotationVector<SourcePrimType_>> {
300  public:
301  inline static RotationVector<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
302  return RotationVector<DestPrimType_>(rotationVector.toImplementation().template cast<DestPrimType_>());
303  }
304 };
305 
306 template<typename DestPrimType_, typename SourcePrimType_>
307 class ConversionTraits<RotationVector<DestPrimType_>, AngleAxis<SourcePrimType_>> {
308  public:
309  inline static RotationVector<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& angleAxis) {
310  const AngleAxis<DestPrimType_> angleAxisDest(angleAxis);
311  return RotationVector<DestPrimType_>(angleAxisDest.angle()*angleAxisDest.axis());
312  }
313 };
314 
315 
316 template<typename DestPrimType_, typename SourcePrimType_>
317 class ConversionTraits<RotationVector<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
318  public:
320  using std::acos;
321  using std::sqrt;
322  using std::atan2;
323 
324  const DestPrimType_ imaginaryVectorNormSquared = DestPrimType_(1.0)-q.real()*q.real();
325 
326  if (imaginaryVectorNormSquared < internal::NumTraits<SourcePrimType_>::dummy_precision()) {
327  if (q.real() > 0.0) {
328  return RotationVector<DestPrimType_>(DestPrimType_(2.0)*q.imaginary().template cast<DestPrimType_>());
329  }
330  else {
331  return RotationVector<DestPrimType_>(DestPrimType_(-2.0)*q.imaginary().template cast<DestPrimType_>());
332  }
333  }
334  const DestPrimType_ imaginaryVectorNorm = q.imaginary().norm();
335  return RotationVector<DestPrimType_>((DestPrimType_(2.0)*atan2(imaginaryVectorNorm, q.real())/imaginaryVectorNorm*q.imaginary()).template cast<DestPrimType_>());
336  }
337 };
338 
339 // Generic conversion
340 template<typename DestPrimType_, typename SourceImplementation_>
341 class ConversionTraits<RotationVector<DestPrimType_>, SourceImplementation_> {
342  public:
343  inline static RotationVector<DestPrimType_> convert(const SourceImplementation_& rotation) {
345  }
346 };
347 
348 template<typename DestPrimType_, typename SourcePrimType_>
349 class ConversionTraits<RotationVector<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
350  public:
351  inline static RotationVector<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& rotationMatrix) {
352  // Note that Eigen converts a rotation matrix to angle-axis via quaternion.
354 
355  }
356 };
357 
358 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
359  * Multiplication Traits
360  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
361 
362 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
363  * Rotation Traits
364  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
365 
366 
367 } // namespace internal
368 } // namespace kindr
369 
370 
RotationVector< double > RotationVectorPD
Passive rotation vector with double primitive type.
RotationVector(const Base &other)
Constructor using Eigen::Matrix<Scalar, 3, 1>.
RotationVector & setIdentity()
Sets the rotation to identity.
RotationVector< PrimType_ > & derived()
Gets the derived rotation. (only for advanced users)
Base Vector
Rotation Vector as 3x1-matrix.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
void setVector(Scalar x, Scalar y, Scalar z)
Sets the rotation vector.
Representation of a generic rotationThis class defines the generic interface for a rotation...
RotationVector & invert()
Inverts the rotation.
const Vector3 & axis() const
Returns the rotation axis.
Definition: AngleAxis.hpp:204
RotationVector & operator=(const RotationBase< OtherDerived_ > &other)
Assignment operator using another rotation.
void setX(Scalar x)
Sets the first entry of the rotation vector.
AngleAxis getUnique() const
Returns a unique angle axis rotation with angle in [0,pi]. This function is used to compare different...
Definition: AngleAxis.hpp:252
RotationVector & setUnique()
RotationVector(Scalar x, Scalar y, Scalar z)
Constructor using three scalars.
RotationVector getUnique() const
Returns a unique rotation vector with norm in [0,pi). This function is used to compare different rota...
void setVector(const Implementation &vector)
Sets the rotation vector.
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
void setZ(Scalar z)
Sets the third entry of the rotation vector.
Scalar z() const
Returns the third entry of the rotation vector.
Scalar angle() const
Returns the rotation angle.
Definition: AngleAxis.hpp:191
const Implementation & toImplementation() const
Cast to the implementation type.
RotationVector & operator()(const RotationBase< OtherDerived_ > &other)
Parenthesis operator to convert from another rotation.
void setY(Scalar y)
Sets the second entry of the rotation vector.
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
RotationVector< float > RotationVectorF
Active rotation vector with float primitive type.
friend std::ostream & operator<<(std::ostream &out, const RotationVector &rotationVector)
Used for printing the object with std::cout.
PrimType_ Scalar
The primitive type. Float/Double.
RotationVector(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
Eigen::Matrix< PrimType_, 3, 1 > Base
The base type.
RotationVector< float > RotationVectorPF
Passive rotation vector with float primitive type.
RotationVector< double > RotationVectorD
Active rotation vector with double primitive type.
Scalar x() const
Returns the first entry of the rotation vector.
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
RotationVector inverted() const
Returns the inverse of the rotation.
RotationVector()
Default constructor using identity rotation.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
Scalar y() const
Returns the second entry of the rotation vector.
const Vector vector() const
Returns the rotation vector.