Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
MotionVector.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
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 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_MATH_MOTIONVECTOR_H_
31 #define _RL_MATH_MOTIONVECTOR_H_
32 
33 #include <Eigen/Core>
34 
35 namespace rl
36 {
37  namespace math
38  {
39  namespace spatial
40  {
41  template< typename Scalar > class ForceVector;
42 
43  template< typename Scalar >
44  class MotionVector
45  {
46  public:
47  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 
49  typedef Scalar ScalarType;
50 
51  typedef typename ::Eigen::Matrix< Scalar, 6, 1 > MatrixType;
52 
53  typedef const MatrixType ConstMatrixType;
54 
55  typedef ::Eigen::Block< MatrixType, 3, 1 > AngularType;
56 
57  typedef const ::Eigen::Block< ConstMatrixType, 3, 1 > ConstAngularType;
58 
59  typedef ::Eigen::Block< MatrixType, 3, 1 > LinearType;
60 
61  typedef const ::Eigen::Block< ConstMatrixType, 3, 1 > ConstLinearType;
62 
64  {
65  }
66 
67  template< typename OtherDerived >
68  MotionVector(const ::Eigen::MatrixBase< OtherDerived >& other) :
69  data(other)
70  {
71  }
72 
73  virtual ~MotionVector()
74  {
75  }
76 
78  {
79  return data.template segment< 3 >(0);
80  }
81 
83  {
84  return data.template segment< 3 >(0);
85  }
86 
87  template< typename OtherScalar >
89 
90  MotionVector cross(const MotionVector& other) const
91  {
92  MotionVector res;
93  res.angular() = angular().cross(other.angular());
94  res.linear() = angular().cross(other.linear()) + linear().cross(other.angular());
95  return res;
96  }
97 
98  template< typename OtherScalar >
99  Scalar dot(const ForceVector< OtherScalar >& other) const;
100 
102  {
103  return data.template segment< 3 >(3);
104  }
105 
107  {
108  return data.template segment< 3 >(3);
109  }
110 
112  {
113  return data;
114  }
115 
116  template< typename OtherDerived >
117  MotionVector& operator=(const ::Eigen::MatrixBase< OtherDerived >& other)
118  {
119  data = other;
120  return *this;
121  }
122 
123  MotionVector operator+(const MotionVector& other) const
124  {
125  MotionVector res;
126  res.angular() = angular() + other.angular();
127  res.linear() = linear() + other.linear();
128  return res;
129  }
130 
131  MotionVector operator-(const MotionVector& other) const
132  {
133  MotionVector res;
134  res.angular() = angular() - other.angular();
135  res.linear() = linear() - other.linear();
136  return res;
137  }
138 
139  template< typename OtherScalar >
140  MotionVector operator*(const OtherScalar& other) const
141  {
142  MotionVector res;
143  res.angular() = angular() * other;
144  res.linear() = linear() * other;
145  return res;
146  }
147 
148  template< typename OtherScalar >
149  MotionVector operator/(const OtherScalar& other) const
150  {
151  MotionVector res;
152  res.angular() = angular() / other;
153  res.linear() = linear() / other;
154  return res;
155  }
156 
157  void setZero()
158  {
159  angular().setZero();
160  linear().setZero();
161  }
162 
163  protected:
164 
165  private:
167  };
168  }
169  }
170 }
171 
172 #endif // _RL_MATH_MOTIONVECTOR_H_