Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
vpForceTwistMatrix.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  * Twist transformation matrix that allows to transform forces from one
33  * frame to an other.
34  *
35 *****************************************************************************/
36 
37 #include <assert.h>
38 #include <sstream>
39 
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpForceTwistMatrix.h>
43 
58 {
59  for (int i = 0; i < 6; ++i) {
60  for (int j = 0; j < 6; ++j) {
61  rowPtrs[i][j] = M.rowPtrs[i][j];
62  }
63  }
64 
65  return *this;
66 }
67 
72 {
73  for (unsigned int i = 0; i < 6; ++i) {
74  for (unsigned int j = 0; j < 6; ++j) {
75  if (i == j) {
76  (*this)[i][j] = 1.0;
77  }
78  else {
79  (*this)[i][j] = 0.0;
80  }
81  }
82  }
83 }
84 
89 
98 
129 {
130  if (full) {
131  build(M);
132  }
133  else {
135  }
136 }
137 
158  : vpArray2D<double>(6, 6)
159 {
160  build(t, thetau);
161 }
162 
180 vpForceTwistMatrix::vpForceTwistMatrix(const vpThetaUVector &thetau) : vpArray2D<double>(6, 6) { build(thetau); }
181 
202  : vpArray2D<double>(6, 6)
203 {
204  build(t, R);
205 }
206 
225 
246 vpForceTwistMatrix::vpForceTwistMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
247  : vpArray2D<double>(6, 6)
248 {
249  vpTranslationVector T(tx, ty, tz);
250  vpThetaUVector tu(tux, tuy, tuz);
251  build(T, tu);
252 }
253 
273 {
274  vpForceTwistMatrix Fout;
275 
276  for (unsigned int i = 0; i < 6; ++i) {
277  for (unsigned int j = 0; j < 6; ++j) {
278  double s = 0;
279  for (unsigned int k = 0; k < 6; ++k) {
280  s += rowPtrs[i][k] * F.rowPtrs[k][j];
281  }
282  Fout[i][j] = s;
283  }
284  }
285  return Fout;
286 }
287 
296 {
297 
298  if (6 != M.getRows()) {
300  "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols()));
301  }
302 
303  unsigned int m_col = M.getCols();
304  vpMatrix p(6, M.getCols());
305  for (unsigned int i = 0; i < 6; ++i) {
306  for (unsigned int j = 0; j < m_col; ++j) {
307  double s = 0;
308  for (unsigned int k = 0; k < 6; ++k) {
309  s += rowPtrs[i][k] * M[k][j];
310  }
311  p[i][j] = s;
312  }
313  }
314  return p;
315 }
316 
362 {
363  vpColVector Hout(6);
364 
365  if (6 != H.getRows()) {
367  "Cannot multiply a (6x6) force/torque twist matrix by "
368  "a %d dimension column vector",
369  H.getRows()));
370  }
371 
372  Hout = 0.0;
373 
374  for (unsigned int i = 0; i < 6; ++i) {
375  for (unsigned int j = 0; j < 6; ++j) {
376  Hout[i] += rowPtrs[i][j] * H[j];
377  }
378  }
379 
380  return Hout;
381 }
382 
383 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
404 {
405  build(t, R);
406  return *this;
407 }
408 
427 {
428  build(R);
429  return *this;
430 }
431 
453 {
454  build(tv, vpRotationMatrix(thetau));
455  return *this;
456 }
457 
477 {
478  build(vpRotationMatrix(thetau));
479  return *this;
480 }
481 
511 {
512  if (full) {
514  }
515  else {
517  }
518 
519  return *this;
520 }
521 #endif
522 
542 {
543  vpMatrix skewaR = t.skew(t) * R;
544 
545  for (unsigned int i = 0; i < 3; ++i) {
546  for (unsigned int j = 0; j < 3; ++j) {
547  (*this)[i][j] = R[i][j];
548  (*this)[i + 3][j + 3] = R[i][j];
549  (*this)[i + 3][j] = skewaR[i][j];
550  }
551  }
552  return *this;
553 }
554 
572 {
573  for (unsigned int i = 0; i < 3; ++i) {
574  for (unsigned int j = 0; j < 3; ++j) {
575  (*this)[i][j] = R[i][j];
576  (*this)[i + 3][j + 3] = R[i][j];
577  (*this)[i + 3][j] = 0;
578  }
579  }
580  return *this;
581 }
582 
603 {
604  build(tv, vpRotationMatrix(thetau));
605  return *this;
606 }
607 
626 {
627  build(vpRotationMatrix(thetau));
628  return *this;
629 }
630 
659 {
660  if (full) {
662  }
663  else {
665  }
666 
667  return *this;
668 }
669 
689 int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
690 {
691  typedef std::string::size_type size_type;
692 
693  unsigned int m = getRows();
694  unsigned int n = getCols();
695 
696  std::vector<std::string> values(m * n);
697  std::ostringstream oss;
698  std::ostringstream ossFixed;
699  std::ios_base::fmtflags original_flags = oss.flags();
700 
701  // --comment: ossFixed less less std fixed
702  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
703 
704  size_type maxBefore = 0; // the length of the integral part
705  size_type maxAfter = 0; // number of decimals plus
706  // one place for the decimal point
707  for (unsigned int i = 0; i < m; ++i) {
708  for (unsigned int j = 0; j < n; ++j) {
709  oss.str("");
710  oss << (*this)[i][j];
711  if (oss.str().find("e") != std::string::npos) {
712  ossFixed.str("");
713  ossFixed << (*this)[i][j];
714  oss.str(ossFixed.str());
715  }
716 
717  values[(i * n) + j] = oss.str();
718  size_type thislen = values[(i * n) + j].size();
719  size_type p = values[(i * n) + j].find('.');
720 
721  if (p == std::string::npos) {
722  maxBefore = vpMath::maximum(maxBefore, thislen);
723  // maxAfter remains the same
724  }
725  else {
726  maxBefore = vpMath::maximum(maxBefore, p);
727  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
728  }
729  }
730  }
731 
732  size_type totalLength = length;
733  // increase totalLength according to maxBefore
734  totalLength = vpMath::maximum(totalLength, maxBefore);
735  // decrease maxAfter according to totalLength
736  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
737  if (maxAfter == 1) {
738  maxAfter = 0;
739  }
740 
741  // the following line is useful for debugging
742  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
743 
744  if (intro) {
745  s << intro;
746  }
747  s << "[" << m << "," << n << "]=\n";
748 
749  for (unsigned int i = 0; i < m; ++i) {
750  s << " ";
751  for (unsigned int j = 0; j < n; ++j) {
752  size_type p = values[(i * n) + j].find('.');
753  s.setf(std::ios::right, std::ios::adjustfield);
754  s.width(static_cast<std::streamsize>(maxBefore));
755  s << values[(i * n) + j].substr(0, p).c_str();
756 
757  if (maxAfter > 0) {
758  s.setf(std::ios::left, std::ios::adjustfield);
759  if (p != std::string::npos) {
760  s.width(static_cast<std::streamsize>(maxAfter));
761  s << values[(i * n) + j].substr(p, maxAfter).c_str();
762  }
763  else {
764  assert(maxAfter > 1);
765  s.width(static_cast<std::streamsize>(maxAfter));
766  s << ".0";
767  }
768  }
769 
770  s << ' ';
771  }
772  s << std::endl;
773  }
774 
775  s.flags(original_flags); // restore s to standard state
776 
777  return static_cast<int>(maxBefore + maxAfter);
778 }
779 
780 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
781 
789 
790 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:126
unsigned int getCols() const
Definition: vpArray2D.h:329
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:133
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
vpArray2D< double > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1141
unsigned int getRows() const
Definition: vpArray2D.h:339
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:70
vpForceTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
vp_deprecated vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const
int print(std::ostream &s, unsigned int length, char const *intro=nullptr) const
vp_deprecated void setIdentity()
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:252
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.