Kindr
Kinematics and Dynamics for Robotics
GlobalAngularVelocity.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  * GlobalAngularVelocity.hpp
30  *
31  * Created on: Jul 25, 2016
32  * Author: Christian Gehring
33  */
34 
35 #pragma once
36 
37 #include <Eigen/Core>
38 
40 #include "kindr/common/common.hpp"
47 
48 namespace kindr {
49 
61 template<typename PrimType_>
62 class GlobalAngularVelocity : public RotationDiffBase<GlobalAngularVelocity<PrimType_>>, public AngularVelocity<PrimType_, 3> {
63  private:
66  typedef AngularVelocity<PrimType_, 3> Base;
67  public:
72  typedef typename Base::Implementation Implementation;
73 
76  typedef PrimType_ Scalar;
77 
80  GlobalAngularVelocity()
81  : Base(Base::Zero()) {
82  }
83 
89  GlobalAngularVelocity(Scalar x, Scalar y, Scalar z)
90  : Base(x, y, z) {
91  }
92 
96  explicit GlobalAngularVelocity(const Base& other)
97  : Base(other) {
98  }
99 
103  GlobalAngularVelocity(const Implementation & vector)
104  : Base(vector) {
105  }
106 
112  template<typename RotationDerived_, typename OtherDerived_>
113  inline explicit GlobalAngularVelocity(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
114  : Base(internal::RotationDiffConversionTraits<GlobalAngularVelocity, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived())){
115  }
116 
117  inline Implementation vector() const {
118  return Base::toImplementation();
119  }
120 
121  inline Implementation& vector() {
122  return Base::toImplementation();
123  }
124 
125 
126  inline Implementation& toImplementation() {
127  return Base::toImplementation();
128  }
129 
130  inline const Implementation& toImplementation() const {
131  return Base::toImplementation();
132  }
133 
137  inline Base& toBase() {
138  return static_cast<Base&>(*this);
139  }
140 
144  inline const Base& toBase() const {
145  return static_cast<const Base&>(*this);
146  }
147 
150  template<typename Other_>
151  GlobalAngularVelocity operator +(const Other_& other) const {
152  return GlobalAngularVelocity(this->toBase() + other.toBase());
153  }
154 
157  template<typename Other_>
158  GlobalAngularVelocity operator -(const Other_& other) const {
159  return GlobalAngularVelocity(this->toBase() - other.toBase());
160  }
161 
164  template<typename PrimTypeFactor_>
165  GlobalAngularVelocity operator *(const PrimTypeFactor_& factor) const {
166  return GlobalAngularVelocity(this->toBase()*factor);
167  }
168 
172  template<typename Other_>
173  GlobalAngularVelocity& operator +=(const Other_& other) {
174  this->toBase() += other.toBase();
175  return *this;
176  }
177 
181  template<typename Other_>
182  GlobalAngularVelocity& operator -=(const Other_& other) {
183  this->toBase() -= other.toBase();
184  return *this;
185  }
186 
190  GlobalAngularVelocity& setZero() {
191  Base::setZero();
192  return *this;
193  }
194  };
195 
196 
199 template<typename PrimType_, typename PrimTypeFactor_>
200 GlobalAngularVelocity<PrimType_> operator *(PrimTypeFactor_ factor, const GlobalAngularVelocity<PrimType_>& globalAngularVelocity) {
201  return GlobalAngularVelocity<PrimType_>(globalAngularVelocity.toBase()*factor);
202 }
203 
205 typedef GlobalAngularVelocity<double> GlobalAngularVelocityD;
207 typedef GlobalAngularVelocity<float> GlobalAngularVelocityF;
208 
209 
210 
211 namespace internal {
212 
213 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
214  * Conversion Traits
215  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
216 template<typename PrimType_, typename Rotation_>
217 class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, LocalAngularVelocity<PrimType_>, Rotation_> {
218  public:
219  inline static GlobalAngularVelocity<PrimType_> convert(const Rotation_& rotationLocalToGlobal, const LocalAngularVelocity<PrimType_>& localAngularVelocity) {
220  return GlobalAngularVelocity<PrimType_>(rotationLocalToGlobal.rotate(localAngularVelocity.toImplementation()));
221  }
222 };
223 
224 
225 template<typename PrimType_>
226 class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, RotationQuaternionDiff<PrimType_>, RotationQuaternion<PrimType_>> {
227  public:
228  inline static GlobalAngularVelocity<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const RotationQuaternionDiff<PrimType_>& rquatdiff) {
229  return GlobalAngularVelocity<PrimType_>(2.0*rquat.getGlobalQuaternionDiffMatrix()*rquatdiff.toQuaternion().vector());
230  }
231 };
232 
233 
234 template<typename PrimType_>
235 class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, RotationMatrixDiff<PrimType_>, RotationMatrix<PrimType_>> {
236  public:
237  inline static GlobalAngularVelocity<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const RotationMatrixDiff<PrimType_>& rotationMatrixDiff) {
238  return GlobalAngularVelocity<PrimType_>(getVectorFromSkewMatrix<PrimType_>(rotationMatrixDiff.matrix()*rotationMatrix.inverted().matrix()));
239  }
240 };
241 
242 
243 template<typename PrimType_>
244 class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, EulerAnglesZyxDiff<PrimType_>, EulerAnglesZyx<PrimType_>> {
245  public:
246  inline static GlobalAngularVelocity<PrimType_> convert(const EulerAnglesZyx<PrimType_>& eulerAngles, const EulerAnglesZyxDiff<PrimType_>& eulerAnglesDiff) {
247  return GlobalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToGlobalAngularVelocity()*eulerAnglesDiff.toImplementation());
248 
249  }
250 };
251 
252 template<typename PrimType_>
253 class RotationDiffConversionTraits<GlobalAngularVelocity<PrimType_>, EulerAnglesXyzDiff<PrimType_>, EulerAnglesXyz<PrimType_>> {
254  public:
255  inline static GlobalAngularVelocity<PrimType_> convert(const EulerAnglesXyz<PrimType_>& eulerAngles, const EulerAnglesXyzDiff<PrimType_>& eulerAnglesDiff) {
256  return GlobalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToGlobalAngularVelocity()*eulerAnglesDiff.toImplementation());
257 
258 
259  }
260 };
261 
262 } // namespace internal
263 } /* namespace kindr */
264 
Vector< PhysicalType_, PrimType_, Dimension_ > operator*(PrimTypeFactor_ factor, const Vector< PhysicalType_, PrimType_, Dimension_ > &vector)
Multiplies a vector with a scalar.
Definition: Vector.hpp:578
Vector< PhysicalType::AngularVelocity, PrimType_, Dimension_ > AngularVelocity
AngularVelocity-Vector.