Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 
36 #include <assert.h>
37 #include <sstream>
38 
39 #include <visp3/core/vpException.h>
40 #include <visp3/core/vpVelocityTwistMatrix.h>
41 
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  for (unsigned int i = 0; i < 6; i++)
72  for (unsigned int j = 0; j < 6; j++)
73  if (i == j)
74  (*this)[i][j] = 1.0;
75  else
76  (*this)[i][j] = 0.0;
77 }
78 
83 
91 
111 {
112  if (full)
113  buildFrom(M);
114  else
116 }
117 
133  : vpArray2D<double>(6, 6)
134 {
135  buildFrom(t, thetau);
136 }
137 
151 {
152  buildFrom(thetau);
153 }
154 
169  : vpArray2D<double>(6, 6)
170 {
171  buildFrom(t, R);
172 }
173 
186 
202 vpVelocityTwistMatrix::vpVelocityTwistMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
203  : vpArray2D<double>(6, 6)
204 {
205  vpTranslationVector t(tx, ty, tz);
206  vpThetaUVector tu(tux, tuy, tuz);
207  buildFrom(t, tu);
208 }
209 
217 {
219 
220  for (unsigned int i = 0; i < 6; i++) {
221  for (unsigned int j = 0; j < 6; j++) {
222  double s = 0;
223  for (int k = 0; k < 6; k++)
224  s += rowPtrs[i][k] * V.rowPtrs[k][j];
225  p[i][j] = s;
226  }
227  }
228  return p;
229 }
230 
271 {
272  if (6 != M.getRows()) {
273  throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
274  M.getRows(), M.getCols()));
275  }
276 
277  vpMatrix p(6, M.getCols());
278  for (unsigned int i = 0; i < 6; i++) {
279  for (unsigned int j = 0; j < M.getCols(); j++) {
280  double s = 0;
281  for (unsigned int k = 0; k < 6; k++)
282  s += rowPtrs[i][k] * M[k][j];
283  p[i][j] = s;
284  }
285  }
286  return p;
287 }
288 
301 {
302  vpColVector c(6);
303 
304  if (6 != v.getRows()) {
306  "Cannot multiply a (6x6) velocity twist matrix by a "
307  "(%d) column vector",
308  v.getRows()));
309  }
310 
311  c = 0.0;
312 
313  for (unsigned int i = 0; i < 6; i++) {
314  for (unsigned int j = 0; j < 6; j++) {
315  {
316  c[i] += rowPtrs[i][j] * v[j];
317  }
318  }
319  }
320 
321  return c;
322 }
323 
336 {
337  for (unsigned int i = 0; i < 3; i++) {
338  for (unsigned int j = 0; j < 3; j++) {
339  (*this)[i][j] = R[i][j];
340  (*this)[i + 3][j + 3] = R[i][j];
341  (*this)[i][j + 3] = 0;
342  }
343  }
344  return (*this);
345 }
346 
362 {
363  vpMatrix skewaR = t.skew(t) * R;
364 
365  for (unsigned int i = 0; i < 3; i++) {
366  for (unsigned int j = 0; j < 3; j++) {
367  (*this)[i][j] = R[i][j];
368  (*this)[i + 3][j + 3] = R[i][j];
369  (*this)[i][j + 3] = skewaR[i][j];
370  }
371  }
372 
373  return (*this);
374 }
375 
392 {
393  buildFrom(t, vpRotationMatrix(thetau));
394  return (*this);
395 }
396 
410 {
411  buildFrom(vpRotationMatrix(thetau));
412  return (*this);
413 }
414 
434 {
435  if (full)
437  else
439 
440  return (*this);
441 }
442 
445 {
448  extract(R);
450  extract(T);
452  RtT = -(R.t() * T);
453 
454  Wi.buildFrom(RtT, R.t());
455 
456  return Wi;
457 }
458 
461 
464 {
465  for (unsigned int i = 0; i < 3; i++)
466  for (unsigned int j = 0; j < 3; j++)
467  R[i][j] = (*this)[i][j];
468 }
469 
472 {
474  extract(R);
475  vpMatrix skTR(3, 3);
476  for (unsigned int i = 0; i < 3; i++)
477  for (unsigned int j = 0; j < 3; j++)
478  skTR[i][j] = (*this)[i][j + 3];
479 
480  vpMatrix skT = skTR * R.t();
481  tv[0] = skT[2][1];
482  tv[1] = skT[0][2];
483  tv[2] = skT[1][0];
484 }
485 
505 int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
506 {
507  typedef std::string::size_type size_type;
508 
509  unsigned int m = getRows();
510  unsigned int n = getCols();
511 
512  std::vector<std::string> values(m * n);
513  std::ostringstream oss;
514  std::ostringstream ossFixed;
515  std::ios_base::fmtflags original_flags = oss.flags();
516 
517  // ossFixed <<std::fixed;
518  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
519 
520  size_type maxBefore = 0; // the length of the integral part
521  size_type maxAfter = 0; // number of decimals plus
522  // one place for the decimal point
523  for (unsigned int i = 0; i < m; ++i) {
524  for (unsigned int j = 0; j < n; ++j) {
525  oss.str("");
526  oss << (*this)[i][j];
527  if (oss.str().find("e") != std::string::npos) {
528  ossFixed.str("");
529  ossFixed << (*this)[i][j];
530  oss.str(ossFixed.str());
531  }
532 
533  values[i * n + j] = oss.str();
534  size_type thislen = values[i * n + j].size();
535  size_type p = values[i * n + j].find('.');
536 
537  if (p == std::string::npos) {
538  maxBefore = vpMath::maximum(maxBefore, thislen);
539  // maxAfter remains the same
540  }
541  else {
542  maxBefore = vpMath::maximum(maxBefore, p);
543  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
544  }
545  }
546  }
547 
548  size_type totalLength = length;
549  // increase totalLength according to maxBefore
550  totalLength = vpMath::maximum(totalLength, maxBefore);
551  // decrease maxAfter according to totalLength
552  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
553  if (maxAfter == 1)
554  maxAfter = 0;
555 
556  // the following line is useful for debugging
557  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
558 
559  if (intro)
560  s << intro;
561  s << "[" << m << "," << n << "]=\n";
562 
563  for (unsigned int i = 0; i < m; i++) {
564  s << " ";
565  for (unsigned int j = 0; j < n; j++) {
566  size_type p = values[i * n + j].find('.');
567  s.setf(std::ios::right, std::ios::adjustfield);
568  s.width((std::streamsize)maxBefore);
569  s << values[i * n + j].substr(0, p).c_str();
570 
571  if (maxAfter > 0) {
572  s.setf(std::ios::left, std::ios::adjustfield);
573  if (p != std::string::npos) {
574  s.width((std::streamsize)maxAfter);
575  s << values[i * n + j].substr(p, maxAfter).c_str();
576  }
577  else {
578  assert(maxAfter > 1);
579  s.width((std::streamsize)maxAfter);
580  s << ".0";
581  }
582  }
583 
584  s << ' ';
585  }
586  s << std::endl;
587  }
588 
589  s.flags(original_flags); // restore s to standard state
590 
591  return (int)(maxBefore + maxAfter);
592 }
593 
594 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
595 
603 
604 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
unsigned int getCols() const
Definition: vpArray2D.h:274
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:132
vpArray2D< double > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1055
unsigned int getRows() const
Definition: vpArray2D.h:284
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:83
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.
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)
vp_deprecated void setIdentity()