Robotics Library  0.6.2
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 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 //
26 
27 #ifndef _RL_MATH_TRANSFORM_H_
28 #define _RL_MATH_TRANSFORM_H_
29 
30 #define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
31 #define EIGEN_QUATERNIONBASE_PLUGIN <rl/math/QuaternionBaseAddons.h>
32 
33 #include <Eigen/Geometry>
34 
35 #include "Matrix.h"
36 #include "Quaternion.h"
37 
38 namespace rl
39 {
40  namespace math
41  {
42  typedef ::Eigen::Transform< Real, 3, ::Eigen::Affine > Transform;
43 
44  typedef ::Eigen::Translation< Real, 3 > Translation;
45 
46  namespace transform
47  {
48  template< typename Matrix1, typename Matrix2, typename Real >
49  inline
50  Real
51  distance(const Matrix1& t1, const Matrix2& t2, const Real& weight = 1.0)
52  {
53  Quaternion q1(t1.rotation());
54  Quaternion q2(t2.rotation());
55 
56  return ::std::sqrt(
57  ::std::pow(t2(0, 3) - t1(0, 3), 2) +
58  ::std::pow(t2(1, 3) - t1(1, 3), 2) +
59  ::std::pow(t2(2, 3) - t1(2, 3), 2) +
60  weight * ::std::pow(q1.angularDistance(q2), 2)
61  );
62  }
63 
64  template< typename Matrix1, typename Real, typename Matrix2 >
65  inline
66  void
67  fromDelta(const Matrix1& t1, const Real& x, const Real& y, const Real& z, const Real& a, const Real& b, const Real& c, Matrix2& t2)
68  {
69 // assert(t1.rows() > 3);
70 // assert(t1.cols() > 3);
71 // assert(t2.rows() > 3);
72 // assert(t2.cols() > 3);
73 
74  t2(0, 0) = t1(0, 0) - c * t1(1, 0) + b * t1(2, 0);
75  t2(0, 1) = t1(0, 1) - c * t1(1, 1) + b * t1(2, 1);
76  t2(0, 2) = t1(0, 2) - c * t1(1, 2) + b * t1(2, 2);
77  t2(0, 3) = t1(0, 3) + x;
78  t2(1, 0) = t1(1, 0) + c * t1(0, 0) - a * t1(2, 0);
79  t2(1, 1) = t1(1, 1) + c * t1(0, 1) - a * t1(2, 1);
80  t2(1, 2) = t1(1, 2) + c * t1(0, 2) - a * t1(2, 2);
81  t2(1, 3) = t1(1, 3) + y;
82  t2(2, 0) = t1(2, 0) - b * t1(0, 0) + a * t1(1, 0);
83  t2(2, 1) = t1(2, 1) - b * t1(0, 1) + a * t1(1, 1);
84  t2(2, 2) = t1(2, 2) - b * t1(0, 2) + a * t1(1, 2);
85  t2(2, 3) = t1(2, 3) + z;
86  t2(3, 0) = 0.0f;
87  t2(3, 1) = 0.0f;
88  t2(3, 2) = 0.0f;
89  t2(3, 3) = 1.0f;
90  }
91 
92  template< typename Matrix1, typename Vector, typename Matrix2 >
93  inline
94  void
95  fromDelta(const Matrix1& t1, const Vector& xyzabc, Matrix2& t2)
96  {
97 // assert(xyzabc.size() > 5);
98 
99  fromDelta(t1, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t2);
100  }
101 
102  template< typename Matrix, typename Real >
103  inline
104  void
105  getDelta(const Matrix& t, Real& x, Real& y, Real& z, Real& a, Real& b, Real& c)
106  {
107 // assert(t.rows() > 3);
108 // assert(t.cols() > 3);
109 
110  x = t(0, 3);
111  y = t(1, 3);
112  z = t(2, 3);
113  a = (t(2, 1) - t(1, 2)) / 2.0f;
114  b = (t(0, 2) - t(2, 0)) / 2.0f;
115  c = (t(1, 0) - t(0, 1)) / 2.0f;
116  }
117 
118  template< typename Matrix, typename Vector >
119  inline
120  void
121  getDelta(const Matrix& t, Vector& xyzabc)
122  {
123 // assert(xyzabc.size() > 5);
124 
125  getDelta(t, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
126  }
127 
128  template< typename Matrix, typename Real >
129  inline
130  void
131  getDenavitHartenberg(const Matrix& t, Real& d, Real& theta, Real& a, Real& alpha)
132  {
133 // assert(t.rows() > 3);
134 // assert(t.cols() > 3);
135 
136  assert(::std::abs(t(2, 0)) <= ::std::numeric_limits<Real>::epsilon());
137 
138  d = t(2, 3);
139 
140  theta = ::std::atan2(t(1, 0), t(0, 0));
141 
142  if (::std::abs(t(0, 0)) <= ::std::numeric_limits<Real>::epsilon())
143  {
144  a = t(1, 3) / t(1, 0);
145  }
146  else
147  {
148  a = t(0, 3) / t(0, 0);
149  }
150 
151  alpha = ::std::atan2(t(2, 1), t(2, 2));
152  }
153 
154  template< typename Real, typename Matrix >
155  inline
156  void
157  setDelta(const Real& x, const Real& y, const Real& z, const Real& a, const Real& b, const Real& c, Matrix& t)
158  {
159 // assert(t.rows() > 3);
160 // assert(t.cols() > 3);
161 
162  t(0, 0) = 0;
163  t(0, 1) = -c;
164  t(0, 2) = b;
165  t(0, 3) = x;
166  t(1, 0) = c;
167  t(1, 1) = 0;
168  t(1, 2) = -a;
169  t(1, 3) = y;
170  t(2, 0) = -b;
171  t(2, 1) = a;
172  t(2, 2) = 0;
173  t(2, 3) = z;
174  t(3, 0) = 0;
175  t(3, 1) = 0;
176  t(3, 2) = 0;
177  t(3, 3) = 1;
178  }
179 
180  template< typename Vector, typename Matrix >
181  inline
182  void
183  setDelta(const Vector& xyzabc, Matrix& t)
184  {
185 // assert(xyzabc.size() > 5);
186 
187  setDelta(xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5), t);
188  }
189 
190  template< typename Real, typename Matrix >
191  inline
192  void
193  setDenavitHartenberg(const Real& d, const Real& theta, const Real& a, const Real& alpha, Matrix& t)
194  {
195 // assert(t.rows() > 3);
196 // assert(t.cols() > 3);
197 
198  Real cosAlpha = ::std::cos(alpha);
199  Real cosTheta = ::std::cos(theta);
200  Real sinAlpha = ::std::sin(alpha);
201  Real sinTheta = ::std::sin(theta);
202 
203  t(0, 0) = cosTheta;
204  t(1, 0) = sinTheta;
205  t(2, 0) = 0.0f;
206  t(3, 0) = 0.0f;
207  t(0, 1) = -cosAlpha * sinTheta;
208  t(1, 1) = cosAlpha * cosTheta;
209  t(2, 1) = sinAlpha;
210  t(3, 1) = 0.0f;
211  t(0, 2) = sinAlpha * sinTheta;
212  t(1, 2) = -sinAlpha * cosTheta;
213  t(2, 2) = cosAlpha;
214  t(3, 2) = 0.0f;
215  t(0, 3) = a * cosTheta;
216  t(1, 3) = a * sinTheta;
217  t(2, 3) = d;
218  t(3, 3) = 1.0f;
219  }
220 
221  template< typename Matrix1, typename Matrix2, typename Real >
222  inline
223  void
224  toDelta(const Matrix1& t1, const Matrix2& t2, Real& x, Real& y, Real& z, Real& a, Real& b, Real& c)
225  {
226 // assert(t1.rows() > 3);
227 // assert(t1.cols() > 3);
228 // assert(t2.rows() > 3);
229 // assert(t2.cols() > 3);
230 
231  x = t2(0, 3) - t1(0, 3);
232  y = t2(1, 3) - t1(1, 3);
233  z = t2(2, 3) - t1(2, 3);
234  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;
235  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;
236  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;
237  }
238 
239  template< typename Matrix1, typename Matrix2, typename Vector >
240  inline
241  void
242  toDelta(const Matrix1& t1, const Matrix2& t2, Vector& xyzabc)
243  {
244 // assert(xyzabc.size() > 5);
245 
246  toDelta(t1, t2, xyzabc(0), xyzabc(1), xyzabc(2), xyzabc(3), xyzabc(4), xyzabc(5));
247  }
248  }
249  }
250 }
251 
252 #endif // _RL_MATH_TRANSFORM_H_
rl::math::Quaternion
::Eigen::Quaternion< Real > Quaternion
Definition: Quaternion.h:41
rl::math::transform::getDelta
void getDelta(const Matrix &t, Real &x, Real &y, Real &z, Real &a, Real &b, Real &c)
Definition: Transform.h:105
rl::math::transform::toDelta
void toDelta(const Matrix1 &t1, const Matrix2 &t2, Real &x, Real &y, Real &z, Real &a, Real &b, Real &c)
Definition: Transform.h:224
Quaternion.h
rl::math::Translation
::Eigen::Translation< Real, 3 > Translation
Definition: Transform.h:44
rl::math::transform::getDenavitHartenberg
void getDenavitHartenberg(const Matrix &t, Real &d, Real &theta, Real &a, Real &alpha)
Definition: Transform.h:131
rl::math::Matrix
::Eigen::Matrix< Real, ::Eigen::Dynamic, ::Eigen::Dynamic > Matrix
Definition: Matrix.h:41
rl::math::transform::distance
Real distance(const Matrix1 &t1, const Matrix2 &t2, const Real &weight=1.0)
Definition: Transform.h:51
Matrix.h
rl::math::Transform
::Eigen::Transform< Real, 3, ::Eigen::Affine > Transform
Definition: Transform.h:42
rl::math::transform::fromDelta
void fromDelta(const Matrix1 &t1, const Real &x, const Real &y, const Real &z, const Real &a, const Real &b, const Real &c, Matrix2 &t2)
Definition: Transform.h:67
rl::math::Vector
::Eigen::Matrix< Real, ::Eigen::Dynamic, 1 > Vector
Definition: Vector.h:41
rl::math::transform::setDenavitHartenberg
void setDenavitHartenberg(const Real &d, const Real &theta, const Real &a, const Real &alpha, Matrix &t)
Definition: Transform.h:193
rl::math::Real
double Real
Definition: Real.h:34
rl::math::transform::setDelta
void setDelta(const Real &x, const Real &y, const Real &z, const Real &a, const Real &b, const Real &c, Matrix &t)
Definition: Transform.h:157
rl
Definition: Ati.cpp:35
Generated by doxygen 1.8.20