Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Transform.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_TRANSFORM_H_
31 #define _RL_MATH_TRANSFORM_H_
32 
33 #define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
34 
35 #include <Eigen/Geometry>
36 
37 #include "Matrix.h"
38 #include "Quaternion.h"
39 
40 namespace rl
41 {
42  namespace math
43  {
44  typedef ::Eigen::Transform< Real, 3, ::Eigen::Affine > Transform;
45 
46  typedef ::Eigen::Translation< Real, 3 > Translation;
47 
48  namespace transform
49  {
50  template< typename Matrix1, typename Matrix2, typename Real >
51  inline
52  Real
53  distance(const Matrix1& t1, const Matrix2& t2, const Real& weight = 1.0)
54  {
55  Quaternion q1(t1.rotation());
56  Quaternion q2(t2.rotation());
57 
58  return ::std::sqrt(
59  ::std::pow(t2(0, 3) - t1(0, 3), 2) +
60  ::std::pow(t2(1, 3) - t1(1, 3), 2) +
61  ::std::pow(t2(2, 3) - t1(2, 3), 2) +
62  weight * ::std::pow(q1.angularDistance(q2), 2)
63  );
64  }
65 
66  template< typename Matrix1, typename Real, typename Matrix2 >
67  inline
68  void
69  fromDelta(const Matrix1& t1, const Real& x, const Real& y, const Real& z, const Real& a, const Real& b, const Real& c, Matrix2& t2)
70  {
71 // assert(t1.rows() > 3);
72 // assert(t1.cols() > 3);
73 // assert(t2.rows() > 3);
74 // assert(t2.cols() > 3);
75 
76  t2(0, 0) = t1(0, 0) - c * t1(1, 0) + b * t1(2, 0);
77  t2(0, 1) = t1(0, 1) - c * t1(1, 1) + b * t1(2, 1);
78  t2(0, 2) = t1(0, 2) - c * t1(1, 2) + b * t1(2, 2);
79  t2(0, 3) = t1(0, 3) + x;
80  t2(1, 0) = t1(1, 0) + c * t1(0, 0) - a * t1(2, 0);
81  t2(1, 1) = t1(1, 1) + c * t1(0, 1) - a * t1(2, 1);
82  t2(1, 2) = t1(1, 2) + c * t1(0, 2) - a * t1(2, 2);
83  t2(1, 3) = t1(1, 3) + y;
84  t2(2, 0) = t1(2, 0) - b * t1(0, 0) + a * t1(1, 0);
85  t2(2, 1) = t1(2, 1) - b * t1(0, 1) + a * t1(1, 1);
86  t2(2, 2) = t1(2, 2) - b * t1(0, 2) + a * t1(1, 2);
87  t2(2, 3) = t1(2, 3) + z;
88  t2(3, 0) = 0.0f;
89  t2(3, 1) = 0.0f;
90  t2(3, 2) = 0.0f;
91  t2(3, 3) = 1.0f;
92  }
93 
94  template< typename Matrix1, typename Vector, typename Matrix2 >
95  inline
96  void
97  fromDelta(const Matrix1& t1, const Vector& xyzabc, Matrix2& t2)
98  {
99 // assert(xyzabc.size() > 5);
100 
101  fromDelta(t1, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t2);
102  }
103 
104  template< typename Matrix, typename Real >
105  inline
106  void
107  getDelta(const Matrix& t, Real& x, Real& y, Real& z, Real& a, Real& b, Real& c)
108  {
109 // assert(t.rows() > 3);
110 // assert(t.cols() > 3);
111 
112  x = t(0, 3);
113  y = t(1, 3);
114  z = t(2, 3);
115  a = (t(2, 1) - t(1, 2)) / 2.0f;
116  b = (t(0, 2) - t(2, 0)) / 2.0f;
117  c = (t(1, 0) - t(0, 1)) / 2.0f;
118  }
119 
120  template< typename Matrix, typename Vector >
121  inline
122  void
123  getDelta(const Matrix& t, Vector& xyzabc)
124  {
125 // assert(xyzabc.size() > 5);
126 
127  getDelta(t, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
128  }
129 
130  template< typename Matrix, typename Real >
131  inline
132  void
133  getDenavitHartenberg(const Matrix& t, Real& d, Real& theta, Real& a, Real& alpha)
134  {
135 // assert(t.rows() > 3);
136 // assert(t.cols() > 3);
137 
138  assert(::std::abs(t(2, 0)) <= ::std::numeric_limits<Real>::epsilon());
139 
140  d = t(2, 3);
141 
142  theta = ::std::atan2(t(1, 0), t(0, 0));
143 
144  if (::std::abs(t(0, 0)) <= ::std::numeric_limits<Real>::epsilon())
145  {
146  a = t(1, 3) / t(1, 0);
147  }
148  else
149  {
150  a = t(0, 3) / t(0, 0);
151  }
152 
153  alpha = ::std::atan2(t(2, 1), t(2, 2));
154  }
155 
156  template< typename Real, typename Matrix >
157  inline
158  void
159  setDelta(const Real& x, const Real& y, const Real& z, const Real& a, const Real& b, const Real& c, Matrix& t)
160  {
161 // assert(t.rows() > 3);
162 // assert(t.cols() > 3);
163 
164  t(0, 0) = 0;
165  t(0, 1) = -c;
166  t(0, 2) = b;
167  t(0, 3) = x;
168  t(1, 0) = c;
169  t(1, 1) = 0;
170  t(1, 2) = -a;
171  t(1, 3) = y;
172  t(2, 0) = -b;
173  t(2, 1) = a;
174  t(2, 2) = 0;
175  t(2, 3) = z;
176  t(3, 0) = 0;
177  t(3, 1) = 0;
178  t(3, 2) = 0;
179  t(3, 3) = 1;
180  }
181 
182  template< typename Vector, typename Matrix >
183  inline
184  void
185  setDelta(const Vector& xyzabc, Matrix& t)
186  {
187 // assert(xyzabc.size() > 5);
188 
189  setDelta(xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t);
190  }
191 
192  template< typename Real, typename Matrix >
193  inline
194  void
195  setDenavitHartenberg(const Real& d, const Real& theta, const Real& a, const Real& alpha, Matrix& t)
196  {
197 // assert(t.rows() > 3);
198 // assert(t.cols() > 3);
199 
200  Real cosAlpha = ::std::cos(alpha);
201  Real cosTheta = ::std::cos(theta);
202  Real sinAlpha = ::std::sin(alpha);
203  Real sinTheta = ::std::sin(theta);
204 
205  t(0, 0) = cosTheta;
206  t(1, 0) = sinTheta;
207  t(2, 0) = 0.0f;
208  t(3, 0) = 0.0f;
209  t(0, 1) = -cosAlpha * sinTheta;
210  t(1, 1) = cosAlpha * cosTheta;
211  t(2, 1) = sinAlpha;
212  t(3, 1) = 0.0f;
213  t(0, 2) = sinAlpha * sinTheta;
214  t(1, 2) = -sinAlpha * cosTheta;
215  t(2, 2) = cosAlpha;
216  t(3, 2) = 0.0f;
217  t(0, 3) = a * cosTheta;
218  t(1, 3) = a * sinTheta;
219  t(2, 3) = d;
220  t(3, 3) = 1.0f;
221  }
222 
223  template< typename Matrix1, typename Matrix2, typename Real >
224  inline
225  void
226  toDelta(const Matrix1& t1, const Matrix2& t2, Real& x, Real& y, Real& z, Real& a, Real& b, Real& c)
227  {
228 // assert(t1.rows() > 3);
229 // assert(t1.cols() > 3);
230 // assert(t2.rows() > 3);
231 // assert(t2.cols() > 3);
232 
233  x = t2(0, 3) - t1(0, 3);
234  y = t2(1, 3) - t1(1, 3);
235  z = t2(2, 3) - t1(2, 3);
236  a = (t1(1, 0) * t2(2, 0) - t1(2, 0) * t2(1, 0) + t1(1, 1) * t2(2, 1) - t1(2, 1) * t2(1, 1) + t1(1, 2) * t2(2, 2) - t1(2, 2) * t2(1, 2)) / 2.0f;
237  b = (t1(2, 0) * t2(0, 0) - t1(0, 0) * t2(2, 0) + t1(2, 1) * t2(0, 1) - t1(0, 1) * t2(2, 1) + t1(2, 2) * t2(0, 2) - t1(0, 2) * t2(2, 2)) / 2.0f;
238  c = (t1(0, 0) * t2(1, 0) - t1(1, 0) * t2(0, 0) + t1(0, 1) * t2(1, 1) - t1(1, 1) * t2(0, 1) + t1(0, 2) * t2(1, 2) - t1(1, 2) * t2(0, 2)) / 2.0f;
239  }
240 
241  template< typename Matrix1, typename Matrix2, typename Vector >
242  inline
243  void
244  toDelta(const Matrix1& t1, const Matrix2& t2, Vector& xyzabc)
245  {
246 // assert(xyzabc.size() > 5);
247 
248  toDelta(t1, t2, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
249  }
250  }
251  }
252 }
253 
254 #endif // _RL_MATH_TRANSFORM_H_