Kindr
Kinematics and Dynamics for Robotics
Twist.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 "kindr/common/common.hpp"
36 
37 namespace kindr {
38 
39 template<typename PrimType_, typename PositionDiff_, typename RotationDiff_>
40 class Twist : public PoseDiffBase<Twist<PrimType_, PositionDiff_, RotationDiff_> >, private PositionDiff_, private RotationDiff_ {
41  public:
42 
43  typedef PrimType_ Scalar;
44  typedef PositionDiff_ PositionDiff;
45  typedef RotationDiff_ RotationDiff;
46  typedef Eigen::Matrix<PrimType_, 6, 1> Vector6;
47  typedef Eigen::Matrix<PrimType_,3, 1> Vector3;
48 
49  Twist() = default;
50 
51  Twist(const PositionDiff& position, const RotationDiff& rotation) :
52  PositionDiff(position),RotationDiff(rotation) {
53  }
54 
55 
56  inline PositionDiff_ & getTranslationalVelocity() {
57  return static_cast<PositionDiff_ &>(*this);
58  }
59 
60  inline const PositionDiff_ & getTranslationalVelocity() const {
61  return static_cast<const PositionDiff_ &>(*this);
62  }
63 
64  inline RotationDiff_ & getRotationalVelocity() {
65  return static_cast<RotationDiff_ &>(*this);
66  }
67 
68  inline const RotationDiff_ & getRotationalVelocity() const {
69  return static_cast<const RotationDiff_ &>(*this);
70  }
71 
72  template<typename Rotation_>
73  inline Vector6 getVector(const RotationBase<Rotation_>& rotation) const {
74  Vector6 vector = Vector6::Zero();
75 
76  vector.template block<3,1>(0,0) = getTranslationalVelocity().toImplementation();
77  vector.template block<3,1>(3,0) = LocalAngularVelocity<Scalar>(rotation.derived(), getRotationalVelocity()).toImplementation();
78  return vector;
79  }
80 
84  friend std::ostream & operator << (std::ostream & out, const Twist & twist) {
85  out << twist.getTranslationalVelocity() << " " << twist.getRotationalVelocity();
86  return out;
87  }
88 
93  PositionDiff::setZero();
94  RotationDiff::setZero();
95  return *this;
96  }
97 };
98 
99 template<typename PrimType_>
101  Velocity<PrimType_, 3>,
102  RotationQuaternionDiff<PrimType_>>
103 {
104  private:
106  public:
107  typedef PrimType_ Scalar;
110 
112 
113  TwistLinearVelocityRotationQuaternionDiff(const PositionDiff& positionDiff, const RotationDiff& rotationDiff):
114  Base(positionDiff, rotationDiff) {
115  }
116 
117 };
118 
119 
122 
123 
124 template<typename PrimType_>
126  Velocity<PrimType_, 3>,
127  LocalAngularVelocity<PrimType_>>
128  {
129  private:
131  public:
132  typedef PrimType_ Scalar;
135  typedef typename Base::Vector6 Vector6;
136  typedef typename Base::Vector3 Vector3;
137 
139 
140  TwistLinearVelocityLocalAngularVelocity(const PositionDiff& positionDiff, const RotationDiff& rotationDiff):
141  Base(positionDiff, rotationDiff) {
142  }
143 
144  TwistLinearVelocityLocalAngularVelocity(const Vector3& linearVelocity, const Vector3& localAngularVelocity):
145  Base(PositionDiff(linearVelocity), RotationDiff(localAngularVelocity)) {
146  }
147 
151  Base(PositionDiff(vector6.template head<3>()), RotationDiff(vector6.template tail<3>())) {
152  }
153 
157  inline Vector6 getVector() const {
158  Vector6 vector = Vector6::Zero();
159  vector.template block<3,1>(0,0) = this->getTranslationalVelocity().toImplementation();
160  vector.template block<3,1>(3,0) = this->getRotationalVelocity().toImplementation();
161  return vector;
162  }
163 
166  inline void setVector(const Vector6& vector6) {
167  this->getTranslationalVelocity().toImplementation() = vector6.template head<3>();
168  this->getRotationalVelocity().toImplementation() = vector6.template tail<3>();
169  }
170 
171 };
172 
179 
180 template<typename PrimType_>
182  Velocity<PrimType_, 3>,
183  GlobalAngularVelocity<PrimType_>>
184  {
185  private:
187  public:
188  typedef PrimType_ Scalar;
191  typedef typename Base::Vector6 Vector6;
192  typedef typename Base::Vector3 Vector3;
193 
195 
196  TwistLinearVelocityGlobalAngularVelocity(const PositionDiff& positionDiff, const RotationDiff& rotationDiff):
197  Base(positionDiff, rotationDiff) {
198  }
199 
200  TwistLinearVelocityGlobalAngularVelocity(const Vector3& linearVelocity, const Vector3& globalAngularVelocity):
201  Base(PositionDiff(linearVelocity), RotationDiff(globalAngularVelocity)) {
202  }
203 
207  Base(PositionDiff(vector6.template head<3>()), RotationDiff(vector6.template tail<3>())) {
208  }
209 
213  inline Vector6 getVector() const {
214  Vector6 vector = Vector6::Zero();
215  vector.template block<3,1>(0,0) = this->getTranslationalVelocity().toImplementation();
216  vector.template block<3,1>(3,0) = this->getRotationalVelocity().toImplementation();
217  return vector;
218  }
219 
222  inline void setVector(const Vector6& vector6) {
223  this->getTranslationalVelocity().toImplementation() = vector6.template head<3>();
224  this->getRotationalVelocity().toImplementation() = vector6.template tail<3>();
225  }
226 
227 };
228 
229 
230 
237 
238 
239 } // namespace kindr
Twist(const PositionDiff &position, const RotationDiff &rotation)
Definition: Twist.hpp:51
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
TwistLinearVelocityLocalAngularVelocity< float > TwistLocalF
Definition: Twist.hpp:178
PrimType_ Scalar
Definition: Twist.hpp:43
TwistLinearVelocityGlobalAngularVelocity(const PositionDiff &positionDiff, const RotationDiff &rotationDiff)
Definition: Twist.hpp:196
TwistLinearVelocityGlobalAngularVelocity< float > TwistLinearVelocityGlobalAngularVelocityPF
Definition: Twist.hpp:234
Representation of a generic rotationThis class defines the generic interface for a rotation...
TwistLinearVelocityGlobalAngularVelocity< float > TwistLinearVelocityGlobalAngularVelocityF
Definition: Twist.hpp:232
RotationDiff_ RotationDiff
Definition: Twist.hpp:45
TwistLinearVelocityLocalAngularVelocity< float > TwistLinearVelocityLocalAngularVelocityF
Definition: Twist.hpp:174
Eigen::Matrix< PrimType_, 6, 1 > Vector6
Definition: Twist.hpp:46
Twist()=default
const PositionDiff_ & getTranslationalVelocity() const
Definition: Twist.hpp:60
void setVector(const Vector6 &vector6)
Definition: Twist.hpp:166
TwistLinearVelocityLocalAngularVelocity< double > TwistLocalD
Definition: Twist.hpp:177
TwistLinearVelocityRotationQuaternionDiff(const PositionDiff &positionDiff, const RotationDiff &rotationDiff)
Definition: Twist.hpp:113
TwistLinearVelocityLocalAngularVelocity< double > TwistLinearVelocityLocalAngularVelocityD
Definition: Twist.hpp:173
TwistLinearVelocityRotationQuaternionDiff< float > TwistLinearVelocityRotationQuaternionDiffF
Definition: Twist.hpp:121
Twist< PrimType_, kindr::Velocity< PrimType_, 3 >, LocalAngularVelocity< PrimType_ > > Base
Definition: Twist.hpp:130
RotationDiff_ & getRotationalVelocity()
Definition: Twist.hpp:64
TwistLinearVelocityLocalAngularVelocity(const PositionDiff &positionDiff, const RotationDiff &rotationDiff)
Definition: Twist.hpp:140
TwistLinearVelocityRotationQuaternionDiff< double > TwistLinearVelocityRotationQuaternionDiffD
Definition: Twist.hpp:120
Twist< PrimType_, kindr::Velocity< PrimType_, 3 >, RotationQuaternionDiff< PrimType_ > > Base
Definition: Twist.hpp:105
friend std::ostream & operator<<(std::ostream &out, const Twist &twist)
Used for printing the object with std::cout.
Definition: Twist.hpp:84
Base class that defines the interface of the time derivative of a pose of a rigid body...
PositionDiff_ PositionDiff
Definition: Twist.hpp:44
TwistLinearVelocityGlobalAngularVelocity< float > TwistGlobalF
Definition: Twist.hpp:236
Twist< PrimType_, kindr::Velocity< PrimType_, 3 >, GlobalAngularVelocity< PrimType_ > > Base
Definition: Twist.hpp:186
void setVector(const Vector6 &vector6)
Definition: Twist.hpp:222
Twist & setZero()
Sets twist to zero.
Definition: Twist.hpp:92
TwistLinearVelocityGlobalAngularVelocity(const Vector3 &linearVelocity, const Vector3 &globalAngularVelocity)
Definition: Twist.hpp:200
const RotationDiff_ & getRotationalVelocity() const
Definition: Twist.hpp:68
PositionDiff_ & getTranslationalVelocity()
Definition: Twist.hpp:56
Eigen::Matrix< PrimType_, 3, 1 > Vector3
Definition: Twist.hpp:47
TwistLinearVelocityGlobalAngularVelocity(const Vector6 &vector6)
Definition: Twist.hpp:206
TwistLinearVelocityLocalAngularVelocity(const Vector6 &vector6)
Definition: Twist.hpp:150
TwistLinearVelocityGlobalAngularVelocity< double > TwistGlobalD
Definition: Twist.hpp:235
Vector6 getVector(const RotationBase< Rotation_ > &rotation) const
Definition: Twist.hpp:73
TwistLinearVelocityLocalAngularVelocity< float > TwistLinearVelocityLocalAngularVelocityPF
Definition: Twist.hpp:176
TwistLinearVelocityGlobalAngularVelocity< double > TwistLinearVelocityGlobalAngularVelocityD
Definition: Twist.hpp:231
TwistLinearVelocityLocalAngularVelocity(const Vector3 &linearVelocity, const Vector3 &localAngularVelocity)
Definition: Twist.hpp:144
TwistLinearVelocityLocalAngularVelocity< double > TwistLinearVelocityLocalAngularVelocityPD
Definition: Twist.hpp:175
Time derivative of a rotation quaternion.
TwistLinearVelocityGlobalAngularVelocity< double > TwistLinearVelocityGlobalAngularVelocityPD
Definition: Twist.hpp:233