Visual Servoing Platform  version 3.6.1 under development (2024-10-10)
vpVelocityTwistMatrix.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 *****************************************************************************/
34 
43 #include <assert.h>
44 #include <sstream>
45 
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 
49 BEGIN_VISP_NAMESPACE
56 {
57  for (int i = 0; i < 6; ++i) {
58  for (int j = 0; j < 6; ++j) {
59  rowPtrs[i][j] = V.rowPtrs[i][j];
60  }
61  }
62 
63  return *this;
64 }
65 
70 {
71  const unsigned int nparam = 6;
72  for (unsigned int i = 0; i < nparam; ++i) {
73  for (unsigned int j = 0; j < nparam; ++j) {
74  if (i == j) {
75  (*this)[i][j] = 1.0;
76  }
77  else {
78  (*this)[i][j] = 0.0;
79  }
80  }
81  }
82 }
83 
88 
96 
116 {
117  if (full) {
118  buildFrom(M);
119  }
120  else {
122  }
123 }
124 
140  : vpArray2D<double>(6, 6)
141 {
142  buildFrom(t, thetau);
143 }
144 
158 {
159  buildFrom(thetau);
160 }
161 
176  : vpArray2D<double>(6, 6)
177 {
178  buildFrom(t, R);
179 }
180 
193 
209 vpVelocityTwistMatrix::vpVelocityTwistMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
210  : vpArray2D<double>(6, 6)
211 {
212  vpTranslationVector t(tx, ty, tz);
213  vpThetaUVector tu(tux, tuy, tuz);
214  buildFrom(t, tu);
215 }
216 
224 {
226  const unsigned int nparam = 6;
227  for (unsigned int i = 0; i < nparam; ++i) {
228  for (unsigned int j = 0; j < nparam; ++j) {
229  double s = 0;
230  for (unsigned int k = 0; k < nparam; ++k) {
231  s += rowPtrs[i][k] * V.rowPtrs[k][j];
232  }
233  p[i][j] = s;
234  }
235  }
236  return p;
237 }
238 
283 {
284  const unsigned int nparam = 6;
285  if (nparam != M.getRows()) {
286  throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
287  M.getRows(), M.getCols()));
288  }
289 
290  vpMatrix p(nparam, M.getCols());
291  unsigned int m_col = M.getCols();
292  for (unsigned int i = 0; i < nparam; ++i) {
293  for (unsigned int j = 0; j < m_col; ++j) {
294  double s = 0;
295  for (unsigned int k = 0; k < nparam; ++k) {
296  s += rowPtrs[i][k] * M[k][j];
297  }
298  p[i][j] = s;
299  }
300  }
301  return p;
302 }
303 
316 {
317  const unsigned int nparam = 6;
318  vpColVector c(nparam);
319 
320  if (nparam != v.getRows()) {
322  "Cannot multiply a (6x6) velocity twist matrix by a "
323  "(%d) column vector",
324  v.getRows()));
325  }
326 
327  c = 0.0;
328 
329  for (unsigned int i = 0; i < nparam; ++i) {
330  for (unsigned int j = 0; j < nparam; ++j) {
331  c[i] += rowPtrs[i][j] * v[j];
332  }
333  }
334 
335  return c;
336 }
337 
350 {
351  const unsigned int val_3 = 3;
352  for (unsigned int i = 0; i < val_3; ++i) {
353  for (unsigned int j = 0; j < val_3; ++j) {
354  (*this)[i][j] = R[i][j];
355  (*this)[i + 3][j + 3] = R[i][j];
356  (*this)[i][j + 3] = 0;
357  }
358  }
359  return *this;
360 }
361 
377 {
378  vpMatrix skewaR = t.skew(t) * R;
379 
380  const unsigned int val_3 = 3;
381  for (unsigned int i = 0; i < val_3; ++i) {
382  for (unsigned int j = 0; j < val_3; ++j) {
383  (*this)[i][j] = R[i][j];
384  (*this)[i + 3][j + 3] = R[i][j];
385  (*this)[i][j + 3] = skewaR[i][j];
386  }
387  }
388 
389  return *this;
390 }
391 
408 {
409  buildFrom(t, vpRotationMatrix(thetau));
410  return *this;
411 }
412 
426 {
427  buildFrom(vpRotationMatrix(thetau));
428  return *this;
429 }
430 
450 {
451  if (full) {
453  }
454  else {
456  }
457 
458  return *this;
459 }
460 
463 {
466  extract(R);
468  extract(T);
470  RtT = -(R.t() * T);
471 
472  Wi.buildFrom(RtT, R.t());
473 
474  return Wi;
475 }
476 
479 
482 {
483  const unsigned int val_3 = 3;
484  for (unsigned int i = 0; i < val_3; ++i) {
485  for (unsigned int j = 0; j < val_3; ++j) {
486  R[i][j] = (*this)[i][j];
487  }
488  }
489 }
490 
493 {
495  extract(R);
496  vpMatrix skTR(3, 3);
497  const unsigned int val_3 = 3;
498  for (unsigned int i = 0; i < val_3; ++i) {
499  for (unsigned int j = 0; j < val_3; ++j) {
500  skTR[i][j] = (*this)[i][j + val_3];
501  }
502  }
503 
504  vpMatrix skT = skTR * R.t();
505  const unsigned int index_0 = 0;
506  const unsigned int index_1 = 1;
507  const unsigned int index_2 = 2;
508  tv[index_0] = skT[index_2][index_1];
509  tv[index_1] = skT[index_0][index_2];
510  tv[index_2] = skT[index_1][index_0];
511 }
512 
532 int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
533 {
534  typedef std::string::size_type size_type;
535 
536  unsigned int m = getRows();
537  unsigned int n = getCols();
538 
539  std::vector<std::string> values(m * n);
540  std::ostringstream oss;
541  std::ostringstream ossFixed;
542  std::ios_base::fmtflags original_flags = oss.flags();
543 
544  // --comment: could be used ossFixed << std::fixed
545  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
546 
547  size_type maxBefore = 0; // the length of the integral part
548  size_type maxAfter = 0; // number of decimals plus
549  // one place for the decimal point
550  for (unsigned int i = 0; i < m; ++i) {
551  for (unsigned int j = 0; j < n; ++j) {
552  oss.str("");
553  oss << (*this)[i][j];
554  if (oss.str().find("e") != std::string::npos) {
555  ossFixed.str("");
556  ossFixed << (*this)[i][j];
557  oss.str(ossFixed.str());
558  }
559 
560  values[(i * n) + j] = oss.str();
561  size_type thislen = values[(i * n) + j].size();
562  size_type p = values[(i * n) + j].find('.');
563 
564  if (p == std::string::npos) {
565  maxBefore = vpMath::maximum(maxBefore, thislen);
566  // maxAfter remains the same
567  }
568  else {
569  maxBefore = vpMath::maximum(maxBefore, p);
570  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
571  }
572  }
573  }
574 
575  size_type totalLength = length;
576  // increase totalLength according to maxBefore
577  totalLength = vpMath::maximum(totalLength, maxBefore);
578  // decrease maxAfter according to totalLength
579  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
580  if (maxAfter == 1) {
581  maxAfter = 0;
582  }
583 
584  // the following line is useful for debugging
585  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
586 
587  if (intro) {
588  s << intro;
589  }
590  s << "[" << m << "," << n << "]=\n";
591 
592  for (unsigned int i = 0; i < m; ++i) {
593  s << " ";
594  for (unsigned int j = 0; j < n; ++j) {
595  size_type p = values[(i * n) + j].find('.');
596  s.setf(std::ios::right, std::ios::adjustfield);
597  s.width(static_cast<std::streamsize>(maxBefore));
598  s << values[(i * n) + j].substr(0, p).c_str();
599 
600  if (maxAfter > 0) {
601  s.setf(std::ios::left, std::ios::adjustfield);
602  if (p != std::string::npos) {
603  s.width(static_cast<std::streamsize>(maxAfter));
604  s << values[(i * n) + j].substr(p, maxAfter).c_str();
605  }
606  else {
607  assert(maxAfter > 1);
608  s.width(static_cast<std::streamsize>(maxAfter));
609  s << ".0";
610  }
611  }
612 
613  s << ' ';
614  }
615  s << std::endl;
616  }
617 
618  s.flags(original_flags); // restore s to standard state
619 
620  return static_cast<int>(maxBefore + maxAfter);
621 }
622 
623 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
624 
631 void vpVelocityTwistMatrix::setIdentity() { eye(); }
632 
633 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
634 END_VISP_NAMESPACE
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1102
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
vpArray2D< double > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1163
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.
vpVelocityTwistMatrix inverse() const
Invert the velocity twist matrix.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)