Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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(double tx, double ty, double tz, double tux, double tuy, double tuz)
206  : vpArray2D<double>(6, 6)
207 {
208  vpTranslationVector t(tx, ty, tz);
209  vpThetaUVector tu(tux, tuy, tuz);
210  buildFrom(t, tu);
211 }
212 
220 {
222 
223  for (unsigned int i = 0; i < 6; i++) {
224  for (unsigned int j = 0; j < 6; j++) {
225  double s = 0;
226  for (int k = 0; k < 6; k++)
227  s += rowPtrs[i][k] * V.rowPtrs[k][j];
228  p[i][j] = s;
229  }
230  }
231  return p;
232 }
233 
274 {
275  if (6 != M.getRows()) {
276  throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
277  M.getRows(), M.getCols()));
278  }
279 
280  vpMatrix p(6, M.getCols());
281  for (unsigned int i = 0; i < 6; i++) {
282  for (unsigned int j = 0; j < M.getCols(); j++) {
283  double s = 0;
284  for (unsigned int k = 0; k < 6; k++)
285  s += rowPtrs[i][k] * M[k][j];
286  p[i][j] = s;
287  }
288  }
289  return p;
290 }
291 
304 {
305  vpColVector c(6);
306 
307  if (6 != v.getRows()) {
309  "Cannot multiply a (6x6) velocity twist matrix by a "
310  "(%d) column vector",
311  v.getRows()));
312  }
313 
314  c = 0.0;
315 
316  for (unsigned int i = 0; i < 6; i++) {
317  for (unsigned int j = 0; j < 6; j++) {
318  {
319  c[i] += rowPtrs[i][j] * v[j];
320  }
321  }
322  }
323 
324  return c;
325 }
326 
339 {
340  for (unsigned int i = 0; i < 3; i++) {
341  for (unsigned int j = 0; j < 3; j++) {
342  (*this)[i][j] = R[i][j];
343  (*this)[i + 3][j + 3] = R[i][j];
344  (*this)[i][j + 3] = 0;
345  }
346  }
347  return (*this);
348 }
349 
365 {
366  vpMatrix skewaR = t.skew(t) * R;
367 
368  for (unsigned int i = 0; i < 3; i++) {
369  for (unsigned int j = 0; j < 3; j++) {
370  (*this)[i][j] = R[i][j];
371  (*this)[i + 3][j + 3] = R[i][j];
372  (*this)[i][j + 3] = skewaR[i][j];
373  }
374  }
375 
376  return (*this);
377 }
378 
395 {
396  buildFrom(t, vpRotationMatrix(thetau));
397  return (*this);
398 }
399 
413 {
414  buildFrom(vpRotationMatrix(thetau));
415  return (*this);
416 }
417 
437 {
438  if (full)
440  else
442 
443  return (*this);
444 }
445 
448 {
451  extract(R);
453  extract(T);
455  RtT = -(R.t() * T);
456 
457  Wi.buildFrom(RtT, R.t());
458 
459  return Wi;
460 }
461 
464 
467 {
468  for (unsigned int i = 0; i < 3; i++)
469  for (unsigned int j = 0; j < 3; j++)
470  R[i][j] = (*this)[i][j];
471 }
472 
475 {
477  extract(R);
478  vpMatrix skTR(3, 3);
479  for (unsigned int i = 0; i < 3; i++)
480  for (unsigned int j = 0; j < 3; j++)
481  skTR[i][j] = (*this)[i][j + 3];
482 
483  vpMatrix skT = skTR * R.t();
484  tv[0] = skT[2][1];
485  tv[1] = skT[0][2];
486  tv[2] = skT[1][0];
487 }
488 
508 int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
509 {
510  typedef std::string::size_type size_type;
511 
512  unsigned int m = getRows();
513  unsigned int n = getCols();
514 
515  std::vector<std::string> values(m * n);
516  std::ostringstream oss;
517  std::ostringstream ossFixed;
518  std::ios_base::fmtflags original_flags = oss.flags();
519 
520  // ossFixed <<std::fixed;
521  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
522 
523  size_type maxBefore = 0; // the length of the integral part
524  size_type maxAfter = 0; // number of decimals plus
525  // one place for the decimal point
526  for (unsigned int i = 0; i < m; ++i) {
527  for (unsigned int j = 0; j < n; ++j) {
528  oss.str("");
529  oss << (*this)[i][j];
530  if (oss.str().find("e") != std::string::npos) {
531  ossFixed.str("");
532  ossFixed << (*this)[i][j];
533  oss.str(ossFixed.str());
534  }
535 
536  values[i * n + j] = oss.str();
537  size_type thislen = values[i * n + j].size();
538  size_type p = values[i * n + j].find('.');
539 
540  if (p == std::string::npos) {
541  maxBefore = vpMath::maximum(maxBefore, thislen);
542  // maxAfter remains the same
543  } else {
544  maxBefore = vpMath::maximum(maxBefore, p);
545  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
546  }
547  }
548  }
549 
550  size_type totalLength = length;
551  // increase totalLength according to maxBefore
552  totalLength = vpMath::maximum(totalLength, maxBefore);
553  // decrease maxAfter according to totalLength
554  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
555  if (maxAfter == 1)
556  maxAfter = 0;
557 
558  // the following line is useful for debugging
559  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
560 
561  if (intro)
562  s << intro;
563  s << "[" << m << "," << n << "]=\n";
564 
565  for (unsigned int i = 0; i < m; i++) {
566  s << " ";
567  for (unsigned int j = 0; j < n; j++) {
568  size_type p = values[i * n + j].find('.');
569  s.setf(std::ios::right, std::ios::adjustfield);
570  s.width((std::streamsize)maxBefore);
571  s << values[i * n + j].substr(0, p).c_str();
572 
573  if (maxAfter > 0) {
574  s.setf(std::ios::left, std::ios::adjustfield);
575  if (p != std::string::npos) {
576  s.width((std::streamsize)maxAfter);
577  s << values[i * n + j].substr(p, maxAfter).c_str();
578  } else {
579  assert(maxAfter > 1);
580  s.width((std::streamsize)maxAfter);
581  s << ".0";
582  }
583  }
584 
585  s << ' ';
586  }
587  s << std::endl;
588  }
589 
590  s.flags(original_flags); // restore s to standard state
591 
592  return (int)(maxBefore + maxAfter);
593 }
594 
595 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
596 
604 
605 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
vpRotationMatrix t() const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
unsigned int getCols() const
Definition: vpArray2D.h:279
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vp_deprecated void setIdentity()
vpTranslationVector getTranslationVector() const
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) const
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
vpRotationMatrix getRotationMatrix() const