Visual Servoing Platform  version 3.6.1 under development (2024-06-19)
vpVelocityTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
44 #include <assert.h>
45 #include <sstream>
46 
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpVelocityTwistMatrix.h>
49 
57 {
58  for (int i = 0; i < 6; ++i) {
59  for (int j = 0; j < 6; ++j) {
60  rowPtrs[i][j] = V.rowPtrs[i][j];
61  }
62  }
63 
64  return *this;
65 }
66 
71 {
72  for (unsigned int i = 0; i < 6; ++i) {
73  for (unsigned int j = 0; j < 6; ++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  build(M);
119  }
120  else {
122  }
123 }
124 
140  : vpArray2D<double>(6, 6)
141 {
142  build(t, thetau);
143 }
144 
158 {
159  build(thetau);
160 }
161 
176  : vpArray2D<double>(6, 6)
177 {
178  build(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  build(t, tu);
215 }
216 
224 {
226 
227  for (unsigned int i = 0; i < 6; ++i) {
228  for (unsigned int j = 0; j < 6; ++j) {
229  double s = 0;
230  for (int k = 0; k < 6; ++k) {
231  s += rowPtrs[i][k] * V.rowPtrs[k][j];
232  }
233  p[i][j] = s;
234  }
235  }
236  return p;
237 }
238 
279 {
280  if (6 != M.getRows()) {
281  throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
282  M.getRows(), M.getCols()));
283  }
284 
285  vpMatrix p(6, M.getCols());
286  unsigned int m_col = M.getCols();
287  for (unsigned int i = 0; i < 6; ++i) {
288  for (unsigned int j = 0; j < m_col; ++j) {
289  double s = 0;
290  for (unsigned int k = 0; k < 6; ++k) {
291  s += rowPtrs[i][k] * M[k][j];
292  }
293  p[i][j] = s;
294  }
295  }
296  return p;
297 }
298 
311 {
312  vpColVector c(6);
313 
314  if (6 != v.getRows()) {
316  "Cannot multiply a (6x6) velocity twist matrix by a "
317  "(%d) column vector",
318  v.getRows()));
319  }
320 
321  c = 0.0;
322 
323  for (unsigned int i = 0; i < 6; ++i) {
324  for (unsigned int j = 0; j < 6; ++j) {
325  c[i] += rowPtrs[i][j] * v[j];
326  }
327  }
328 
329  return c;
330 }
331 
332 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
345 {
346  build(R);
347  return *this;
348 }
349 
365 {
366  build(t, R);
367  return *this;
368 }
369 
386 {
387  build(t, thetau);
388  return *this;
389 }
390 
404 {
405  build(thetau);
406  return *this;
407 }
408 
428 {
429  build(M, full);
430  return *this;
431 }
432 #endif
433 
446 {
447  for (unsigned int i = 0; i < 3; ++i) {
448  for (unsigned int j = 0; j < 3; ++j) {
449  (*this)[i][j] = R[i][j];
450  (*this)[i + 3][j + 3] = R[i][j];
451  (*this)[i][j + 3] = 0;
452  }
453  }
454  return *this;
455 }
456 
472 {
473  vpMatrix skewaR = t.skew(t) * R;
474 
475  for (unsigned int i = 0; i < 3; ++i) {
476  for (unsigned int j = 0; j < 3; ++j) {
477  (*this)[i][j] = R[i][j];
478  (*this)[i + 3][j + 3] = R[i][j];
479  (*this)[i][j + 3] = skewaR[i][j];
480  }
481  }
482 
483  return *this;
484 }
485 
502 {
503  build(t, vpRotationMatrix(thetau));
504  return *this;
505 }
506 
520 {
521  build(vpRotationMatrix(thetau));
522  return *this;
523 }
524 
544 {
545  if (full) {
547  }
548  else {
550  }
551 
552  return *this;
553 }
554 
557 {
560  extract(R);
562  extract(T);
564  RtT = -(R.t() * T);
565 
566  Wi.build(RtT, R.t());
567 
568  return Wi;
569 }
570 
573 
576 {
577  for (unsigned int i = 0; i < 3; ++i) {
578  for (unsigned int j = 0; j < 3; ++j) {
579  R[i][j] = (*this)[i][j];
580  }
581  }
582 }
583 
586 {
588  extract(R);
589  vpMatrix skTR(3, 3);
590  for (unsigned int i = 0; i < 3; ++i) {
591  for (unsigned int j = 0; j < 3; ++j) {
592  skTR[i][j] = (*this)[i][j + 3];
593  }
594  }
595 
596  vpMatrix skT = skTR * R.t();
597  tv[0] = skT[2][1];
598  tv[1] = skT[0][2];
599  tv[2] = skT[1][0];
600 }
601 
621 int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
622 {
623  typedef std::string::size_type size_type;
624 
625  unsigned int m = getRows();
626  unsigned int n = getCols();
627 
628  std::vector<std::string> values(m * n);
629  std::ostringstream oss;
630  std::ostringstream ossFixed;
631  std::ios_base::fmtflags original_flags = oss.flags();
632 
633  // --comment: could be used ossFixed << std::fixed
634  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
635 
636  size_type maxBefore = 0; // the length of the integral part
637  size_type maxAfter = 0; // number of decimals plus
638  // one place for the decimal point
639  for (unsigned int i = 0; i < m; ++i) {
640  for (unsigned int j = 0; j < n; ++j) {
641  oss.str("");
642  oss << (*this)[i][j];
643  if (oss.str().find("e") != std::string::npos) {
644  ossFixed.str("");
645  ossFixed << (*this)[i][j];
646  oss.str(ossFixed.str());
647  }
648 
649  values[(i * n) + j] = oss.str();
650  size_type thislen = values[(i * n) + j].size();
651  size_type p = values[(i * n) + j].find('.');
652 
653  if (p == std::string::npos) {
654  maxBefore = vpMath::maximum(maxBefore, thislen);
655  // maxAfter remains the same
656  }
657  else {
658  maxBefore = vpMath::maximum(maxBefore, p);
659  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
660  }
661  }
662  }
663 
664  size_type totalLength = length;
665  // increase totalLength according to maxBefore
666  totalLength = vpMath::maximum(totalLength, maxBefore);
667  // decrease maxAfter according to totalLength
668  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
669  if (maxAfter == 1) {
670  maxAfter = 0;
671  }
672 
673  // the following line is useful for debugging
674  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
675 
676  if (intro) {
677  s << intro;
678  }
679  s << "[" << m << "," << n << "]=\n";
680 
681  for (unsigned int i = 0; i < m; ++i) {
682  s << " ";
683  for (unsigned int j = 0; j < n; ++j) {
684  size_type p = values[(i * n) + j].find('.');
685  s.setf(std::ios::right, std::ios::adjustfield);
686  s.width(static_cast<std::streamsize>(maxBefore));
687  s << values[(i * n) + j].substr(0, p).c_str();
688 
689  if (maxAfter > 0) {
690  s.setf(std::ios::left, std::ios::adjustfield);
691  if (p != std::string::npos) {
692  s.width(static_cast<std::streamsize>(maxAfter));
693  s << values[(i * n) + j].substr(p, maxAfter).c_str();
694  }
695  else {
696  assert(maxAfter > 1);
697  s.width(static_cast<std::streamsize>(maxAfter));
698  s << ".0";
699  }
700  }
701 
702  s << ' ';
703  }
704  s << std::endl;
705  }
706 
707  s.flags(original_flags); // restore s to standard state
708 
709  return static_cast<int>(maxBefore + maxAfter);
710 }
711 
712 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
713 
721 
722 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:127
unsigned int getCols() const
Definition: vpArray2D.h:330
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:342
vpArray2D< double > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1143
unsigned int getRows() const
Definition: vpArray2D.h:340
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
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:151
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 & build(const vpTranslationVector &t, const vpRotationMatrix &R)
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
vp_deprecated vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vp_deprecated void setIdentity()
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)