Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpVelocityTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Velocity twist transformation matrix.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <assert.h>
40 #include <sstream>
41 
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpVelocityTwistMatrix.h>
44 
59 {
60  for (int i = 0; i < 6; i++) {
61  for (int j = 0; j < 6; j++) {
62  rowPtrs[i][j] = V.rowPtrs[i][j];
63  }
64  }
65 
66  return *this;
67 }
68 
73 {
74  for (unsigned int i = 0; i < 6; i++)
75  for (unsigned int j = 0; j < 6; j++)
76  if (i == j)
77  (*this)[i][j] = 1.0;
78  else
79  (*this)[i][j] = 0.0;
80 }
81 
86 
94 
114 {
115  if (full)
116  buildFrom(M);
117  else
119 }
120 
136  : vpArray2D<double>(6, 6)
137 {
138  buildFrom(t, thetau);
139 }
140 
154 {
155  buildFrom(thetau);
156 }
157 
172  : vpArray2D<double>(6, 6)
173 {
174  buildFrom(t, R);
175 }
176 
189 
205 vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx, const double ty, const double tz, const double tux,
206  const double tuy, const double tuz)
207  : vpArray2D<double>(6, 6)
208 {
209  vpTranslationVector t(tx, ty, tz);
210  vpThetaUVector tu(tux, tuy, tuz);
211  buildFrom(t, tu);
212 }
213 
221 {
223 
224  for (unsigned int i = 0; i < 6; i++) {
225  for (unsigned int j = 0; j < 6; j++) {
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  }
232  return p;
233 }
234 
275 {
276  if (6 != M.getRows()) {
277  throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
278  M.getRows(), M.getCols()));
279  }
280 
281  vpMatrix p(6, M.getCols());
282  for (unsigned int i = 0; i < 6; i++) {
283  for (unsigned int j = 0; j < M.getCols(); j++) {
284  double s = 0;
285  for (unsigned int k = 0; k < 6; k++)
286  s += rowPtrs[i][k] * M[k][j];
287  p[i][j] = s;
288  }
289  }
290  return p;
291 }
292 
305 {
306  vpColVector c(6);
307 
308  if (6 != v.getRows()) {
310  "Cannot multiply a (6x6) velocity twist matrix by a "
311  "(%d) column vector",
312  v.getRows()));
313  }
314 
315  c = 0.0;
316 
317  for (unsigned int i = 0; i < 6; i++) {
318  for (unsigned int j = 0; j < 6; j++) {
319  {
320  c[i] += rowPtrs[i][j] * v[j];
321  }
322  }
323  }
324 
325  return c;
326 }
327 
340 {
341  for (unsigned int i = 0; i < 3; i++) {
342  for (unsigned int j = 0; j < 3; j++) {
343  (*this)[i][j] = R[i][j];
344  (*this)[i + 3][j + 3] = R[i][j];
345  (*this)[i][j + 3] = 0;
346  }
347  }
348  return (*this);
349 }
350 
366 {
367  vpMatrix skewaR = t.skew(t) * R;
368 
369  for (unsigned int i = 0; i < 3; i++) {
370  for (unsigned int j = 0; j < 3; j++) {
371  (*this)[i][j] = R[i][j];
372  (*this)[i + 3][j + 3] = R[i][j];
373  (*this)[i][j + 3] = skewaR[i][j];
374  }
375  }
376 
377  return (*this);
378 }
379 
396 {
397  buildFrom(t, vpRotationMatrix(thetau));
398  return (*this);
399 }
400 
414 {
415  buildFrom(vpRotationMatrix(thetau));
416  return (*this);
417 }
418 
438 {
439  if (full)
441  else
443 
444  return (*this);
445 }
446 
449 {
452  extract(R);
454  extract(T);
456  RtT = -(R.t() * T);
457 
458  Wi.buildFrom(RtT, R.t());
459 
460  return Wi;
461 }
462 
465 
468 {
469  for (unsigned int i = 0; i < 3; i++)
470  for (unsigned int j = 0; j < 3; j++)
471  R[i][j] = (*this)[i][j];
472 }
473 
476 {
478  extract(R);
479  vpMatrix skTR(3, 3);
480  for (unsigned int i = 0; i < 3; i++)
481  for (unsigned int j = 0; j < 3; j++)
482  skTR[i][j] = (*this)[i][j + 3];
483 
484  vpMatrix skT = skTR * R.t();
485  tv[0] = skT[2][1];
486  tv[1] = skT[0][2];
487  tv[2] = skT[1][0];
488 }
489 
509 int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
510 {
511  typedef std::string::size_type size_type;
512 
513  unsigned int m = getRows();
514  unsigned int n = getCols();
515 
516  std::vector<std::string> values(m * n);
517  std::ostringstream oss;
518  std::ostringstream ossFixed;
519  std::ios_base::fmtflags original_flags = oss.flags();
520 
521  // ossFixed <<std::fixed;
522  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
523 
524  size_type maxBefore = 0; // the length of the integral part
525  size_type maxAfter = 0; // number of decimals plus
526  // one place for the decimal point
527  for (unsigned int i = 0; i < m; ++i) {
528  for (unsigned int j = 0; j < n; ++j) {
529  oss.str("");
530  oss << (*this)[i][j];
531  if (oss.str().find("e") != std::string::npos) {
532  ossFixed.str("");
533  ossFixed << (*this)[i][j];
534  oss.str(ossFixed.str());
535  }
536 
537  values[i * n + j] = oss.str();
538  size_type thislen = values[i * n + j].size();
539  size_type p = values[i * n + j].find('.');
540 
541  if (p == std::string::npos) {
542  maxBefore = vpMath::maximum(maxBefore, thislen);
543  // maxAfter remains the same
544  } else {
545  maxBefore = vpMath::maximum(maxBefore, p);
546  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
547  }
548  }
549  }
550 
551  size_type totalLength = length;
552  // increase totalLength according to maxBefore
553  totalLength = vpMath::maximum(totalLength, maxBefore);
554  // decrease maxAfter according to totalLength
555  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
556  if (maxAfter == 1)
557  maxAfter = 0;
558 
559  // the following line is useful for debugging
560  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
561 
562  if (intro)
563  s << intro;
564  s << "[" << m << "," << n << "]=\n";
565 
566  for (unsigned int i = 0; i < m; i++) {
567  s << " ";
568  for (unsigned int j = 0; j < n; j++) {
569  size_type p = values[i * n + j].find('.');
570  s.setf(std::ios::right, std::ios::adjustfield);
571  s.width((std::streamsize)maxBefore);
572  s << values[i * n + j].substr(0, p).c_str();
573 
574  if (maxAfter > 0) {
575  s.setf(std::ios::left, std::ios::adjustfield);
576  if (p != std::string::npos) {
577  s.width((std::streamsize)maxAfter);
578  s << values[i * n + j].substr(p, maxAfter).c_str();
579  } else {
580  assert(maxAfter > 1);
581  s.width((std::streamsize)maxAfter);
582  s << ".0";
583  }
584  }
585 
586  s << ' ';
587  }
588  s << std::endl;
589  }
590 
591  s.flags(original_flags); // restore s to standard state
592 
593  return (int)(maxBefore + maxAfter);
594 }
595 
596 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
597 
605 
606 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
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:71
vpRotationMatrix t() const
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
unsigned int getCols() const
Definition: vpArray2D.h:279
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
Implementation of a rotation matrix and operations on such kind of matrices.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getRows() const
Definition: vpArray2D.h:289
vp_deprecated void setIdentity()
vpVelocityTwistMatrix inverse() const
Invert the velocity twist matrix.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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:139
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.