Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
PlueckerTransform.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_PLUECKERTRANSFORM_H_
31 #define _RL_MATH_PLUECKERTRANSFORM_H_
32 
33 #include <Eigen/Core>
34 #include <Eigen/Geometry>
35 
36 namespace rl
37 {
38  namespace math
39  {
40  namespace spatial
41  {
42  template< typename Scalar > class ArticulatedBodyInertia;
43  template< typename Scalar > class ForceVector;
44  template< typename Scalar > class MotionVector;
45  template< typename Scalar > class RigidBodyInertia;
46 
47  template< typename Scalar >
49  {
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef Scalar ScalarType;
54 
55  typedef ::Eigen::Matrix< Scalar, 6, 6 > MatrixType;
56 
57  typedef ::Eigen::Matrix< Scalar, 3, 3 > RotationType;
58 
60 
61  typedef ::Eigen::Transform< Scalar, 3, ::Eigen::Affine > TransformType;
62 
63  typedef ::Eigen::Matrix< Scalar, 3, 1 > TranslationType;
64 
66 
68  {
69  }
70 
72  {
73  }
74 
76  {
78  res.rotation() = rotation().transpose();
79  res.translation() = -rotation() * translation();
80  return res;
81  }
82 
84  {
85  MatrixType res;
86  res.template topLeftCorner< 3, 3 >() = rotation().transpose();
87  res.template topRightCorner< 3, 3 >() = translation().cross33() * rotation().transpose();
88  res.template bottomLeftCorner< 3, 3 >().setZero();
89  res.template bottomRightCorner< 3, 3 >() = rotation().transpose();
90  return res;
91  }
92 
94  {
95  MatrixType res;
96  res.template topLeftCorner< 3, 3 >() = rotation().transpose();
97  res.template topRightCorner< 3, 3 >().setZero();
98  res.template bottomLeftCorner< 3, 3 >() = translation().cross33() * rotation().transpose();
99  res.template bottomRightCorner< 3, 3 >() = rotation().transpose();
100  return res;
101  }
102 
104  {
105  MatrixType res;
106  res.template topLeftCorner< 3, 3 >() = rotation();
107  res.template topRightCorner< 3, 3 >() = -rotation() * translation().cross33();
108  res.template bottomLeftCorner< 3, 3 >().setZero();
109  res.template bottomRightCorner< 3, 3 >() = rotation();
110  return res;
111  }
112 
114  {
115  MatrixType res;
116  res.template topLeftCorner< 3, 3 >() = rotation();
117  res.template topRightCorner< 3, 3 >().setZero();
118  res.template bottomLeftCorner< 3, 3 >() = -rotation() * translation().cross33();
119  res.template bottomRightCorner< 3, 3 >() = rotation();
120  return res;
121  }
122 
123  template< typename OtherScalar >
125 
126  template< typename OtherScalar >
128 
130  {
131  PlueckerTransform res;
132  res.rotation() = rotation() * other.rotation();
133  res.translation() = other.translation() + other.rotation().transpose() * translation();
134  return res;
135  }
136 
137  template< typename OtherScalar >
139 
140  template< typename OtherScalar >
142 
143  template< typename OtherScalar >
145 
146  template< typename OtherScalar >
148 
149  template< typename OtherScalar >
151 
152  template< typename OtherScalar >
154 
156  {
157  return rotationData;
158  }
159 
161  {
162  return rotationData;
163  }
164 
165  void setIdentity()
166  {
167  rotation().setIdentity();
168  translation().setZero();
169  }
170 
172  {
173  TransformType res;
174  res.linear() = rotation();
175  res.translation() = -rotation() * translation();
176 //res.linear() = rotation().transpose();
177 //res.translation() = translation();
178  return res;
179  }
180 
182  {
183  return translationData;
184  }
185 
187  {
188  return translationData;
189  }
190 
191  protected:
192 
193  private:
195 
197  };
198  }
199  }
200 }
201 
202 #endif // _RL_MATH_PLUECKERTRANSFORM_H_