Kindr
Kinematics and Dynamics for Robotics
LocalAngularVelocity.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 <Eigen/Core>
32 
34 #include "kindr/common/common.hpp"
41 
42 namespace kindr {
43 
55 template<typename PrimType_>
56 class LocalAngularVelocity : public RotationDiffBase<LocalAngularVelocity<PrimType_>>, public AngularVelocity<PrimType_, 3> {
57  private:
60  typedef AngularVelocity<PrimType_, 3> Base;
61  public:
66  typedef typename Base::Implementation Implementation;
67 
70  typedef PrimType_ Scalar;
71 
74  LocalAngularVelocity()
75  : Base(Base::Zero()) {
76  }
77 
83  LocalAngularVelocity(Scalar x, Scalar y, Scalar z)
84  : Base(x, y, z) {
85  }
86 
90  explicit LocalAngularVelocity(const Base& other)
91  : Base(other) {
92  }
93 
97  LocalAngularVelocity(const Implementation & vector)
98  : Base(vector) {
99  }
100 
106  template<typename RotationDerived_, typename OtherDerived_>
107  inline explicit LocalAngularVelocity(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
108  : Base(internal::RotationDiffConversionTraits<LocalAngularVelocity, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived())){
109  }
110 
111  inline Implementation vector() const {
112  return Base::toImplementation();
113  }
114 
115  inline Implementation& vector() {
116  return Base::toImplementation();
117  }
118 
119 
120  inline Implementation& toImplementation() {
121  return Base::toImplementation();
122  }
123 
124  inline const Implementation& toImplementation() const {
125  return Base::toImplementation();
126  }
127 
131  inline Base& toBase() {
132  return static_cast<Base&>(*this);
133  }
134 
138  inline const Base& toBase() const {
139  return static_cast<const Base&>(*this);
140  }
141 
144  template<typename Other_>
145  LocalAngularVelocity operator +(const Other_& other) const {
146  return LocalAngularVelocity(this->toBase() + other.toBase());
147  }
148 
151  template<typename Other_>
152  LocalAngularVelocity operator -(const Other_& other) const {
153  return LocalAngularVelocity(this->toBase() - other.toBase());
154  }
155 
158  template<typename PrimTypeFactor_>
159  LocalAngularVelocity operator *(const PrimTypeFactor_& factor) const {
160  return LocalAngularVelocity(this->toBase()*factor);
161  }
162 
166  template<typename Other_>
167  LocalAngularVelocity& operator +=(const Other_& other) {
168  this->toBase() += other.toBase();
169  return *this;
170  }
171 
175  template<typename Other_>
176  LocalAngularVelocity& operator -=(const Other_& other) {
177  this->toBase() -= other.toBase();
178  return *this;
179  }
180 
184  LocalAngularVelocity& setZero() {
185  Base::setZero();
186  return *this;
187  }
188 };
189 
192 template<typename PrimType_, typename PrimTypeFactor_>
193 LocalAngularVelocity<PrimType_> operator *(PrimTypeFactor_ factor, const LocalAngularVelocity<PrimType_>& localAngularVelocity) {
194  return LocalAngularVelocity<PrimType_>(localAngularVelocity.toBase()*factor);
195 }
196 
198 typedef LocalAngularVelocity<double> LocalAngularVelocityPD;
200 typedef LocalAngularVelocity<float> LocalAngularVelocityPF;
202 typedef LocalAngularVelocity<double> LocalAngularVelocityD;
204 typedef LocalAngularVelocity<float> LocalAngularVelocityF;
205 
206 
207 
208 namespace internal {
209 
210 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
211  * Conversion Traits
212  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
213 template<typename PrimType_, typename Rotation_>
214 class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, GlobalAngularVelocity<PrimType_>, Rotation_> {
215  public:
216  inline static LocalAngularVelocity<PrimType_> convert(const Rotation_& rotationLocalToGlobal, const GlobalAngularVelocity<PrimType_>& globalAngularVelocity) {
217  return LocalAngularVelocity<PrimType_>(rotationLocalToGlobal.inverseRotate(globalAngularVelocity.toImplementation()));
218  }
219 };
220 
221 template<typename PrimType_>
222 class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, RotationQuaternionDiff<PrimType_>, RotationQuaternion<PrimType_>> {
223  public:
224  inline static LocalAngularVelocity<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const RotationQuaternionDiff<PrimType_>& rquatdiff) {
225  return LocalAngularVelocity<PrimType_>(2.0*rquat.getLocalQuaternionDiffMatrix()*rquatdiff.toQuaternion().vector());
226  }
227 };
228 
230 template<typename PrimType_>
231 class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, RotationMatrixDiff<PrimType_>, RotationMatrix<PrimType_>> {
232  public:
233  inline static LocalAngularVelocity<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const RotationMatrixDiff<PrimType_>& rotationMatrixDiff) {
234  return LocalAngularVelocity<PrimType_>(getVectorFromSkewMatrix<PrimType_>(rotationMatrix.inverted().matrix()*rotationMatrixDiff.matrix()));
235  }
236 };
237 
238 
239 
240 template<typename PrimType_>
241 class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, EulerAnglesZyxDiff<PrimType_>, EulerAnglesZyx<PrimType_>> {
242  public:
243  inline static LocalAngularVelocity<PrimType_> convert(const EulerAnglesZyx<PrimType_>& eulerAngles, const EulerAnglesZyxDiff<PrimType_>& eulerAnglesDiff) {
244  return LocalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToLocalAngularVelocity()*eulerAnglesDiff.toImplementation());
245 
246  }
247 };
248 
249 template<typename PrimType_>
250 class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, EulerAnglesXyzDiff<PrimType_>, EulerAnglesXyz<PrimType_>> {
251  public:
252  inline static LocalAngularVelocity<PrimType_> convert(const EulerAnglesXyz<PrimType_>& eulerAngles, const EulerAnglesXyzDiff<PrimType_>& eulerAnglesDiff) {
253  return LocalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToLocalAngularVelocity()*eulerAnglesDiff.toImplementation());
254 
255 
256  }
257 };
258 
259 } // namespace internal
260 } // namespace kindr
261 
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.