Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpVelocityTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Velocity twist transformation matrix.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 #include <sstream>
39 #include <assert.h>
40 
41 #include <visp3/core/vpVelocityTwistMatrix.h>
42 #include <visp3/core/vpException.h>
43 
44 
61 {
62  for (int i=0; i<6; i++)
63  {
64  for (int j=0; j<6; j++)
65  {
66  rowPtrs[i][j] = V.rowPtrs[i][j];
67  }
68  }
69 
70  return *this;
71 }
72 
73 
77 void
79 {
80  for (unsigned int i=0 ; i < 6 ; i++)
81  for (unsigned int j=0 ; j < 6; j++)
82  if (i==j)
83  (*this)[i][j] = 1.0 ;
84  else
85  (*this)[i][j] = 0.0;
86 }
87 
92  : vpArray2D<double>(6, 6)
93 {
94  eye() ;
95 }
96 
103  : vpArray2D<double>(6, 6)
104 {
105  *this = V;
106 }
107 
119  : vpArray2D<double>(6, 6)
120 {
121  buildFrom(M);
122 }
123 
135  const vpThetaUVector &thetau)
136  : vpArray2D<double>(6, 6)
137 {
138  buildFrom(t, thetau) ;
139 }
140 
152  const vpRotationMatrix &R)
153  : vpArray2D<double>(6, 6)
154 {
155  buildFrom(t,R) ;
156 }
157 
169  const double ty,
170  const double tz,
171  const double tux,
172  const double tuy,
173  const double tuz)
174  : vpArray2D<double>(6, 6)
175 {
176  vpTranslationVector T(tx,ty,tz) ;
177  vpThetaUVector tu(tux,tuy,tuz) ;
178  buildFrom(T,tu) ;
179 }
180 
189 {
191 
192  for (unsigned int i=0;i<6;i++)
193  for (unsigned int j=0;j<6;j++)
194  {
195  double s =0 ;
196  for (int k=0;k<6;k++)
197  s +=rowPtrs[i][k] * V.rowPtrs[k][j];
198  p[i][j] = s ;
199  }
200  return p;
201 }
202 
239 vpMatrix
241 {
242  if (6 != M.getRows()) {
244  "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
245  M.getRows(), M.getCols()));
246  }
247 
248  vpMatrix p(6, M.getCols()) ;
249  for (unsigned int i=0;i<6;i++)
250  for (unsigned int j=0;j<M.getCols();j++)
251  {
252  double s =0 ;
253  for (unsigned int k=0;k<6;k++)
254  s += rowPtrs[i][k] * M[k][j];
255  p[i][j] = s ;
256  }
257  return p;
258 }
259 
272 {
273  vpColVector c(6);
274 
275  if (6 != v.getRows())
276  {
278  "Cannot multiply a (6x6) velocity twist matrix by a (%d) column vector",
279  v.getRows()));
280  }
281 
282  c = 0.0;
283 
284  for (unsigned int i=0;i<6;i++) {
285  for (unsigned int j=0;j<6;j++) {
286  {
287  c[i]+=rowPtrs[i][j] * v[j];
288  }
289  }
290  }
291 
292  return c ;
293 }
294 
295 
308  const vpRotationMatrix &R)
309 {
310  unsigned int i, j;
311  vpMatrix skewaR = t.skew(t)*R ;
312 
313  for (i=0 ; i < 3 ; i++)
314  for (j=0 ; j < 3 ; j++)
315  {
316  (*this)[i][j] = R[i][j] ;
317  (*this)[i+3][j+3] = R[i][j] ;
318  (*this)[i][j+3] = skewaR[i][j] ;
319 
320  }
321  return (*this) ;
322 }
323 
336  const vpThetaUVector &thetau)
337 {
338  vpRotationMatrix R ;
339  R.buildFrom(thetau) ;
340  buildFrom(t,R) ;
341  return (*this) ;
342 }
343 
344 
357 {
359  vpRotationMatrix R ;
360  M.extract(R) ;
361  M.extract(tv) ;
362 
363  buildFrom(tv, R) ;
364  return (*this) ;
365 }
366 
367 
371 {
375  vpTranslationVector RtT ; RtT = -(R.t()*T) ;
376 
377  Wi.buildFrom(RtT,R.t());
378 
379  return Wi ;
380 }
381 
382 
384 void
386 {
387  V = inverse();
388 }
389 
391 void
393 {
394  for (unsigned int i=0 ; i < 3 ; i++)
395  for (unsigned int j=0 ; j < 3; j++)
396  R[i][j] = (*this)[i][j];
397 }
398 
400 void
402 {
404  vpMatrix skTR(3,3);
405  for (unsigned int i=0 ; i < 3 ; i++)
406  for (unsigned int j=0 ; j < 3; j++)
407  skTR[i][j] = (*this)[i][j+3];
408 
409  vpMatrix skT = skTR*R.t();
410  tv[0] = skT[2][1];
411  tv[1] = skT[0][2];
412  tv[2] = skT[1][0];
413 }
414 
434 int
435 vpVelocityTwistMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
436 {
437  typedef std::string::size_type size_type;
438 
439  unsigned int m = getRows();
440  unsigned int n = getCols();
441 
442  std::vector<std::string> values(m*n);
443  std::ostringstream oss;
444  std::ostringstream ossFixed;
445  std::ios_base::fmtflags original_flags = oss.flags();
446 
447  // ossFixed <<std::fixed;
448  ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
449 
450  size_type maxBefore=0; // the length of the integral part
451  size_type maxAfter=0; // number of decimals plus
452  // one place for the decimal point
453  for (unsigned int i=0;i<m;++i) {
454  for (unsigned int j=0;j<n;++j){
455  oss.str("");
456  oss << (*this)[i][j];
457  if (oss.str().find("e")!=std::string::npos){
458  ossFixed.str("");
459  ossFixed << (*this)[i][j];
460  oss.str(ossFixed.str());
461  }
462 
463  values[i*n+j]=oss.str();
464  size_type thislen=values[i*n+j].size();
465  size_type p=values[i*n+j].find('.');
466 
467  if (p==std::string::npos){
468  maxBefore=vpMath::maximum(maxBefore, thislen);
469  // maxAfter remains the same
470  } else{
471  maxBefore=vpMath::maximum(maxBefore, p);
472  maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
473  }
474  }
475  }
476 
477  size_type totalLength=length;
478  // increase totalLength according to maxBefore
479  totalLength=vpMath::maximum(totalLength,maxBefore);
480  // decrease maxAfter according to totalLength
481  maxAfter=std::min(maxAfter, totalLength-maxBefore);
482  if (maxAfter==1) maxAfter=0;
483 
484  // the following line is useful for debugging
485  //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
486 
487  if (intro) s <<intro;
488  s <<"["<<m<<","<<n<<"]=\n";
489 
490  for (unsigned int i=0;i<m;i++) {
491  s <<" ";
492  for (unsigned int j=0;j<n;j++){
493  size_type p=values[i*n+j].find('.');
494  s.setf(std::ios::right, std::ios::adjustfield);
495  s.width((std::streamsize)maxBefore);
496  s <<values[i*n+j].substr(0,p).c_str();
497 
498  if (maxAfter>0){
499  s.setf(std::ios::left, std::ios::adjustfield);
500  if (p!=std::string::npos){
501  s.width((std::streamsize)maxAfter);
502  s <<values[i*n+j].substr(p,maxAfter).c_str();
503  } else{
504  assert(maxAfter>1);
505  s.width((std::streamsize)maxAfter);
506  s <<".0";
507  }
508  }
509 
510  s <<' ';
511  }
512  s <<std::endl;
513  }
514 
515  s.flags(original_flags); // restore s to standard state
516 
517  return (int)(maxBefore+maxAfter);
518 }
519 
520 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
521 
528 void
530 {
531  eye() ;
532 }
533 
534 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
Implementation of an homogeneous matrix and operations on such kind of matrices.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpRotationMatrix t() const
Implementation of a generic 2D array used as vase class of matrices and vectors.
Definition: vpArray2D.h:70
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
Implementation of a rotation matrix and operations on such kind of matrices.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
void extract(vpRotationMatrix &R) const
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vp_deprecated void setIdentity()
vpVelocityTwistMatrix inverse() const
Invert the velocity twist matrix.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:78
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.