ViSP  2.10.0
vpVelocityTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * $Id: vpVelocityTwistMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Velocity twist transformation matrix.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpVelocityTwistMatrix.h>
43 
44 // Exception
45 #include <visp/vpException.h>
46 #include <visp/vpMatrixException.h>
47 
48 // Debug trace
49 #include <visp/vpDebug.h>
50 
51 
68 {
69  init() ;
70 
71  for (int i=0; i<6; i++)
72  {
73  for (int j=0; j<6; j++)
74  {
75  rowPtrs[i][j] = V.rowPtrs[i][j];
76  }
77  }
78 
79  return *this;
80 }
81 
82 
87 void
89 {
90  unsigned int i,j ;
91 
92  try {
93  resize(6,6) ;
94  }
95  catch(vpException me)
96  {
97  vpERROR_TRACE("Error caught") ;
98  throw ;
99  }
100 
101  for (i=0 ; i < 6 ; i++)
102  for (j=0 ; j < 6; j++)
103  if (i==j)
104  (*this)[i][j] = 1.0 ;
105  else
106  (*this)[i][j] = 0.0;
107 }
108 
113 {
114  init() ;
115 }
116 
123 {
124  init() ;
125  *this = V;
126 }
127 
139 {
140  init() ;
141  buildFrom(M);
142 }
143 
155  const vpThetaUVector &thetau) : vpMatrix()
156 {
157  init() ;
158  buildFrom(tv, thetau) ;
159 }
160 
172  const vpRotationMatrix &R)
173 {
174  init() ;
175  buildFrom(tv,R) ;
176 }
177 
188  const double ty,
189  const double tz,
190  const double tux,
191  const double tuy,
192  const double tuz) : vpMatrix()
193 {
194  init() ;
195  vpTranslationVector T(tx,ty,tz) ;
196  vpThetaUVector tu(tux,tuy,tuz) ;
197  buildFrom(T,tu) ;
198 }
199 
205 void
207 {
208  init() ;
209 }
210 
211 
220 {
222 
223  for (unsigned int i=0;i<6;i++)
224  for (unsigned int j=0;j<6;j++)
225  {
226  double s =0 ;
227  for (int k=0;k<6;k++)
228  s +=rowPtrs[i][k] * V.rowPtrs[k][j];
229  p[i][j] = s ;
230  }
231  return p;
232 }
233 
274 vpMatrix
276 {
277 
278  if (6 != M.getRows())
279  {
280  vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vpMatrix multiply") ;
282  }
283 
284  vpMatrix p(6, M.getCols()) ;
285  for (unsigned int i=0;i<6;i++)
286  for (unsigned int j=0;j<M.getCols();j++)
287  {
288  double s =0 ;
289  for (unsigned int k=0;k<6;k++)
290  s += rowPtrs[i][k] * M[k][j];
291  p[i][j] = s ;
292  }
293  return p;
294 }
295 
312 {
313  vpColVector c(6);
314 
315  if (6 != v.getRows())
316  {
317  vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply") ;
319  "Mismatch in vpVelocityTwistMatrix/vector multiply")) ;
320  }
321 
322  c = 0.0;
323 
324  for (unsigned int i=0;i<6;i++) {
325  for (unsigned int j=0;j<6;j++) {
326  {
327  c[i]+=rowPtrs[i][j] * v[j];
328  }
329  }
330  }
331 
332  return c ;
333 }
334 
335 
348  const vpRotationMatrix &R)
349 {
350  unsigned int i, j;
351  vpMatrix skewaR = tv.skew(tv)*R ;
352 
353  for (i=0 ; i < 3 ; i++)
354  for (j=0 ; j < 3 ; j++)
355  {
356  (*this)[i][j] = R[i][j] ;
357  (*this)[i+3][j+3] = R[i][j] ;
358  (*this)[i][j+3] = skewaR[i][j] ;
359 
360  }
361  return (*this) ;
362 }
363 
376  const vpThetaUVector &thetau)
377 {
378  vpRotationMatrix R ;
379  R.buildFrom(thetau) ;
380  buildFrom(tv,R) ;
381  return (*this) ;
382 
383 }
384 
385 
398 {
400  vpRotationMatrix R ;
401  M.extract(R) ;
402  M.extract(tv) ;
403 
404  buildFrom(tv, R) ;
405  return (*this) ;
406 }
407 
408 
412 {
416  vpTranslationVector RtT ; RtT = -(R.t()*T) ;
417 
418  Wi.buildFrom(RtT,R.t());
419 
420  return Wi ;
421 }
422 
423 
425 void
427 {
428  Wi = inverse();
429 }
430 
432 void
434 {
435  for (unsigned int i=0 ; i < 3 ; i++)
436  for (unsigned int j=0 ; j < 3; j++)
437  R[i][j] = (*this)[i][j];
438 }
439 
441 void
443 {
445  vpMatrix skTR(3,3);
446  for (unsigned int i=0 ; i < 3 ; i++)
447  for (unsigned int j=0 ; j < 3; j++)
448  skTR[i][j] = (*this)[i][j+3];
449 
450  vpMatrix skT = skTR*R.t();
451  tv[0] = skT[2][1];
452  tv[1] = skT[0][2];
453  tv[2] = skT[1][0];
454 }
455 
456 /*
457  * Local variables:
458  * c-basic-offset: 2
459  * End:
460  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
error that can be emited by ViSP classes.
Definition: vpException.h:76
vpRotationMatrix t() const
transpose
The vpRotationMatrix considers the particular case of a rotation matrix.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
double ** rowPtrs
address of the first element of each rows
Definition: vpMatrix.h:121
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Build a rotation matrix from an homogeneous matrix.
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpVelocityTwistMatrix inverse() const
invert the twist matrix
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:163
error that can be emited by the vpMatrix class and its derivates
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void extract(vpRotationMatrix &R) const
extract the rotational matrix from the twist matrix