ROBOOP, A Robotics Object Oriented Package in C++
homogen.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau, Professeur
23 Departement de genie electrique
24 Ecole Polytechnique de Montreal
25 C.P. 6079, Succ. Centre-Ville
26 Montreal, Quebec, H3C 3A7
27 
28 email: richard.gourdeau@polymtl.ca
29 
30 -------------------------------------------------------------------------------
31 Revision_history:
32 
33 2004/07/01: Etienne Lachance
34  -Added protection on trans function.
35  -Added doxygen documentation.
36 
37 2004/07/01: Ethan Tira-Thompson
38  -Added support for newmat's use_namespace #define, using ROBOOP namespace
39 
40 2004/08/10: Etienne Lachance
41  -Make sure input vector is 3x1 in trans function.
42 
43 2006/01/21: Etienne Lachance
44  -Include utils.h instead of robot.h.
45 
46 2006/11/15: Richard Gourdeau
47  -atan2() in irotk()
48 -------------------------------------------------------------------------------
49 */
50 
56 #include "utils.h"
57 
58 #ifdef use_namespace
59 namespace ROBOOP {
60  using namespace NEWMAT;
61 #endif
62 
63 
64 ReturnMatrix trans(const ColumnVector & a)
66 {
67  Matrix translation(4,4);
68 
69  translation << fourbyfourident; // identity matrix
70 
71  if (a.Nrows() == 3)
72  {
73  translation(1,4) = a(1);
74  translation(2,4) = a(2);
75  translation(3,4) = a(3);
76  }
77  else
78  cerr << "trans: wrong size in input vector." << endl;
79 
80  translation.Release(); return translation;
81 }
82 
83 ReturnMatrix rotx(const Real alpha)
85 {
86  Matrix rot(4,4);
87  Real c, s;
88 
89  rot << fourbyfourident; // identity matrix
90 
91  c = cos(alpha);
92  s = sin(alpha);
93 
94  rot(2,2) = c;
95  rot(3,3) = c;
96  rot(2,3) = -s;
97  rot(3,2) = s;
98 
99  rot.Release(); return rot;
100 }
101 
102 
103 ReturnMatrix roty(const Real beta)
105 {
106  Matrix rot(4,4);
107  Real c, s;
108 
109  rot << fourbyfourident; // identity matrix
110 
111  c = cos(beta);
112  s = sin(beta);
113 
114  rot(1,1) = c;
115  rot(3,3) = c;
116  rot(1,3) = s;
117  rot(3,1) = -s;
118 
119  rot.Release(); return rot;
120 }
121 
122 
123 ReturnMatrix rotz(const Real gamma)
125 {
126  Matrix rot(4,4);
127  Real c, s;
128 
129  rot << fourbyfourident; // identity matrix
130 
131  c = cos(gamma);
132  s = sin(gamma);
133 
134  rot(1,1) = c;
135  rot(2,2) = c;
136  rot(1,2) = -s;
137  rot(2,1) = s;
138 
139  rot.Release(); return rot;
140 }
141 
142 // compound rotations
143 
144 
145 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
147 {
148  Matrix rot(4,4);
149  Real c, s, vers, kx, ky, kz;
150 
151  rot << fourbyfourident; // identity matrix
152 
153  vers = SumSquare(k.SubMatrix(1,3,1,1));
154  if (vers != 0.0) { // compute the rotation if the vector norm is not 0.0
155  vers = sqrt(1/vers);
156  kx = k(1)*vers;
157  ky = k(2)*vers;
158  kz = k(3)*vers;
159  s = sin(theta);
160  c = cos(theta);
161  vers = 1-c;
162 
163  rot(1,1) = kx*kx*vers+c;
164  rot(1,2) = kx*ky*vers-kz*s;
165  rot(1,3) = kx*kz*vers+ky*s;
166  rot(2,1) = kx*ky*vers+kz*s;
167  rot(2,2) = ky*ky*vers+c;
168  rot(2,3) = ky*kz*vers-kx*s;
169  rot(3,1) = kx*kz*vers-ky*s;
170  rot(3,2) = ky*kz*vers+kx*s;
171  rot(3,3) = kz*kz*vers+c;
172  }
173 
174  rot.Release(); return rot;
175 }
176 
177 
178 ReturnMatrix rpy(const ColumnVector & a)
180 {
181  Matrix rot(4,4);
182  Real ca, sa, cb, sb, cc, sc;
183 
184  rot << fourbyfourident; // identity matrix
185 
186  ca = cos(a(1));
187  sa = sin(a(1));
188  cb = cos(a(2));
189  sb = sin(a(2));
190  cc = cos(a(3));
191  sc = sin(a(3));
192 
193  rot(1,1) = cb*cc;
194  rot(1,2) = sa*sb*cc-ca*sc;
195  rot(1,3) = ca*sb*cc+sa*sc;
196  rot(2,1) = cb*sc;
197  rot(2,2) = sa*sb*sc+ca*cc;
198  rot(2,3) = ca*sb*sc-sa*cc;
199  rot(3,1) = -sb;
200  rot(3,2) = sa*cb;
201  rot(3,3) = ca*cb;
202 
203  rot.Release(); return rot;
204 }
205 
206 
207 ReturnMatrix eulzxz(const ColumnVector & a)
209 {
210  Matrix rot(4,4);
211  Real c1, s1, ca, sa, c2, s2;
212 
213  rot << fourbyfourident; // identity matrix
214 
215  c1 = cos(a(1));
216  s1 = sin(a(1));
217  ca = cos(a(2));
218  sa = sin(a(2));
219  c2 = cos(a(3));
220  s2 = sin(a(3));
221 
222  rot(1,1) = c1*c2-s1*ca*s2;
223  rot(1,2) = -c1*s2-s1*ca*c2;
224  rot(1,3) = sa*s1;
225  rot(2,1) = s1*c2+c1*ca*s2;
226  rot(2,2) = -s1*s2+c1*ca*c2;
227  rot(2,3) = -sa*c1;
228  rot(3,1) = sa*s2;
229  rot(3,2) = sa*c2;
230  rot(3,3) = ca;
231 
232  rot.Release(); return rot;
233 }
234 
235 
236 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
237  const ColumnVector & k2)
239 {
240  Matrix rot;
241 
242  rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
243 
244  rot.Release(); return rot;
245 }
246 
247 // inverse problem for compound rotations
248 
249 ReturnMatrix irotk(const Matrix & R)
251 {
252  ColumnVector k(4);
253  Real a, b, c;
254 
255  a = (R(3,2)-R(2,3));
256  b = (R(1,3)-R(3,1));
257  c = (R(2,1)-R(1,2));
258  k(4) = atan2(sqrt(a*a + b*b + c*c),(R(1,1) + R(2,2) + R(3,3)-1));
259  k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
260  k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
261  k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
262 
263  k.Release(); return k;
264 }
265 
266 
267 ReturnMatrix irpy(const Matrix & R)
269 {
270  ColumnVector k(3);
271 
272  if (R(3,1)==1) {
273  k(1) = atan2(-R(1,2),-R(1,3));
274  k(2) = -M_PI/2;
275  k(3) = 0.0;
276  } else if (R(3,1)==-1) {
277  k(1) = atan2(R(1,2),R(1,3));
278  k(2) = M_PI/2;
279  k(3) = 0.0;
280  } else {
281  k(1) = atan2(R(3,2), R(3,3));
282  k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
283  k(3) = atan2(R(2,1), R(1,1));
284  }
285 
286  k.Release(); return k;
287 }
288 
289 
290 ReturnMatrix ieulzxz(const Matrix & R)
292 {
293  ColumnVector a(3);
294 
295  if ((R(3,3)==1) || (R(3,3)==-1)) {
296  a(1) = 0.0;
297  a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
298  a(3) = atan2(R(2,1),R(1,1));
299  } else {
300  a(1) = atan2(R(1,3), -R(2,3));
301  a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
302  a(3) = atan2(R(3,1), R(3,2));
303  }
304 
305  a.Release(); return a;
306 }
307 
308 #ifdef use_namespace
309 }
310 #endif