Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpForceTwistMatrix.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  * Twist transformation matrix that allows to transform forces from one
33  * frame to an other.
34  *
35  * Authors:
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <assert.h>
41 #include <sstream>
42 
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpException.h>
45 #include <visp3/core/vpForceTwistMatrix.h>
46 
61 {
62  for (int i = 0; i < 6; i++) {
63  for (int j = 0; j < 6; j++) {
64  rowPtrs[i][j] = M.rowPtrs[i][j];
65  }
66  }
67 
68  return *this;
69 }
70 
75 {
76  for (unsigned int i = 0; i < 6; i++) {
77  for (unsigned int j = 0; j < 6; j++) {
78  if (i == j)
79  (*this)[i][j] = 1.0;
80  else
81  (*this)[i][j] = 0.0;
82  }
83  }
84 }
85 
90 
99 
130 {
131  if (full)
132  buildFrom(M);
133  else
135 }
136 
157  : vpArray2D<double>(6, 6)
158 {
159  buildFrom(t, thetau);
160 }
161 
180 
201  : vpArray2D<double>(6, 6)
202 {
203  buildFrom(t, R);
204 }
205 
224 
245 vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz, const double tux,
246  const double tuy, const double tuz)
247  : vpArray2D<double>(6, 6)
248 {
249  vpTranslationVector T(tx, ty, tz);
250  vpThetaUVector tu(tux, tuy, tuz);
251  buildFrom(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  Fout[i][j] = s;
282  }
283  }
284  return Fout;
285 }
286 
295 {
296 
297  if (6 != M.getRows()) {
299  "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols()));
300  }
301 
302  vpMatrix p(6, M.getCols());
303  for (unsigned int i = 0; i < 6; i++) {
304  for (unsigned int j = 0; j < M.getCols(); j++) {
305  double s = 0;
306  for (unsigned int k = 0; k < 6; k++)
307  s += rowPtrs[i][k] * M[k][j];
308  p[i][j] = s;
309  }
310  }
311  return p;
312 }
313 
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 
403 {
404  vpMatrix skewaR = t.skew(t) * R;
405 
406  for (unsigned int i = 0; i < 3; i++) {
407  for (unsigned int j = 0; j < 3; j++) {
408  (*this)[i][j] = R[i][j];
409  (*this)[i + 3][j + 3] = R[i][j];
410  (*this)[i + 3][j] = skewaR[i][j];
411  }
412  }
413  return (*this);
414 }
415 
434 {
435  for (unsigned int i = 0; i < 3; i++) {
436  for (unsigned int j = 0; j < 3; j++) {
437  (*this)[i][j] = R[i][j];
438  (*this)[i + 3][j + 3] = R[i][j];
439  (*this)[i + 3][j] = 0;
440  }
441  }
442  return (*this);
443 }
444 
466 {
467  buildFrom(tv, vpRotationMatrix(thetau));
468  return (*this);
469 }
470 
490 {
491  buildFrom(vpRotationMatrix(thetau));
492  return (*this);
493 }
494 
524 {
525  if (full)
527  else
529 
530  return (*this);
531 }
532 
552 int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
553 {
554  typedef std::string::size_type size_type;
555 
556  unsigned int m = getRows();
557  unsigned int n = getCols();
558 
559  std::vector<std::string> values(m * n);
560  std::ostringstream oss;
561  std::ostringstream ossFixed;
562  std::ios_base::fmtflags original_flags = oss.flags();
563 
564  // ossFixed <<std::fixed;
565  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
566 
567  size_type maxBefore = 0; // the length of the integral part
568  size_type maxAfter = 0; // number of decimals plus
569  // one place for the decimal point
570  for (unsigned int i = 0; i < m; ++i) {
571  for (unsigned int j = 0; j < n; ++j) {
572  oss.str("");
573  oss << (*this)[i][j];
574  if (oss.str().find("e") != std::string::npos) {
575  ossFixed.str("");
576  ossFixed << (*this)[i][j];
577  oss.str(ossFixed.str());
578  }
579 
580  values[i * n + j] = oss.str();
581  size_type thislen = values[i * n + j].size();
582  size_type p = values[i * n + j].find('.');
583 
584  if (p == std::string::npos) {
585  maxBefore = vpMath::maximum(maxBefore, thislen);
586  // maxAfter remains the same
587  } else {
588  maxBefore = vpMath::maximum(maxBefore, p);
589  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
590  }
591  }
592  }
593 
594  size_type totalLength = length;
595  // increase totalLength according to maxBefore
596  totalLength = vpMath::maximum(totalLength, maxBefore);
597  // decrease maxAfter according to totalLength
598  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
599  if (maxAfter == 1)
600  maxAfter = 0;
601 
602  // the following line is useful for debugging
603  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
604 
605  if (intro)
606  s << intro;
607  s << "[" << m << "," << n << "]=\n";
608 
609  for (unsigned int i = 0; i < m; i++) {
610  s << " ";
611  for (unsigned int j = 0; j < n; j++) {
612  size_type p = values[i * n + j].find('.');
613  s.setf(std::ios::right, std::ios::adjustfield);
614  s.width((std::streamsize)maxBefore);
615  s << values[i * n + j].substr(0, p).c_str();
616 
617  if (maxAfter > 0) {
618  s.setf(std::ios::left, std::ios::adjustfield);
619  if (p != std::string::npos) {
620  s.width((std::streamsize)maxAfter);
621  s << values[i * n + j].substr(p, maxAfter).c_str();
622  } else {
623  assert(maxAfter > 1);
624  s.width((std::streamsize)maxAfter);
625  s << ".0";
626  }
627  }
628 
629  s << ' ';
630  }
631  s << std::endl;
632  }
633 
634  s.flags(original_flags); // restore s to standard state
635 
636  return (int)(maxBefore + maxAfter);
637 }
638 
639 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
640 
648 
649 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
error that can be emited by ViSP classes.
Definition: vpException.h:71
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
vp_deprecated void setIdentity()
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.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
int print(std::ostream &s, unsigned int length, char const *intro=0) const
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