Visual Servoing Platform version 3.5.0
vpVelocityTwistMatrix.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 * Velocity twist transformation matrix.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <assert.h>
40#include <sstream>
41
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpVelocityTwistMatrix.h>
44
59{
60 for (int i = 0; i < 6; i++) {
61 for (int j = 0; j < 6; j++) {
62 rowPtrs[i][j] = V.rowPtrs[i][j];
63 }
64 }
65
66 return *this;
67}
68
73{
74 for (unsigned int i = 0; i < 6; i++)
75 for (unsigned int j = 0; j < 6; j++)
76 if (i == j)
77 (*this)[i][j] = 1.0;
78 else
79 (*this)[i][j] = 0.0;
80}
81
86
94
114{
115 if (full)
116 buildFrom(M);
117 else
119}
120
136 : vpArray2D<double>(6, 6)
137{
138 buildFrom(t, thetau);
139}
140
154{
155 buildFrom(thetau);
156}
157
172 : vpArray2D<double>(6, 6)
173{
174 buildFrom(t, R);
175}
176
189
205vpVelocityTwistMatrix::vpVelocityTwistMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
206 : vpArray2D<double>(6, 6)
207{
208 vpTranslationVector t(tx, ty, tz);
209 vpThetaUVector tu(tux, tuy, tuz);
210 buildFrom(t, tu);
211}
212
220{
222
223 for (unsigned int i = 0; i < 6; i++) {
224 for (unsigned int j = 0; j < 6; j++) {
225 double s = 0;
226 for (int k = 0; k < 6; k++)
227 s += rowPtrs[i][k] * V.rowPtrs[k][j];
228 p[i][j] = s;
229 }
230 }
231 return p;
232}
233
274{
275 if (6 != M.getRows()) {
276 throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
277 M.getRows(), M.getCols()));
278 }
279
280 vpMatrix p(6, M.getCols());
281 for (unsigned int i = 0; i < 6; i++) {
282 for (unsigned int j = 0; j < M.getCols(); j++) {
283 double s = 0;
284 for (unsigned int k = 0; k < 6; k++)
285 s += rowPtrs[i][k] * M[k][j];
286 p[i][j] = s;
287 }
288 }
289 return p;
290}
291
304{
305 vpColVector c(6);
306
307 if (6 != v.getRows()) {
309 "Cannot multiply a (6x6) velocity twist matrix by a "
310 "(%d) column vector",
311 v.getRows()));
312 }
313
314 c = 0.0;
315
316 for (unsigned int i = 0; i < 6; i++) {
317 for (unsigned int j = 0; j < 6; j++) {
318 {
319 c[i] += rowPtrs[i][j] * v[j];
320 }
321 }
322 }
323
324 return c;
325}
326
339{
340 for (unsigned int i = 0; i < 3; i++) {
341 for (unsigned int j = 0; j < 3; j++) {
342 (*this)[i][j] = R[i][j];
343 (*this)[i + 3][j + 3] = R[i][j];
344 (*this)[i][j + 3] = 0;
345 }
346 }
347 return (*this);
348}
349
365{
366 vpMatrix skewaR = t.skew(t) * R;
367
368 for (unsigned int i = 0; i < 3; i++) {
369 for (unsigned int j = 0; j < 3; j++) {
370 (*this)[i][j] = R[i][j];
371 (*this)[i + 3][j + 3] = R[i][j];
372 (*this)[i][j + 3] = skewaR[i][j];
373 }
374 }
375
376 return (*this);
377}
378
395{
396 buildFrom(t, vpRotationMatrix(thetau));
397 return (*this);
398}
399
413{
415 return (*this);
416}
417
437{
438 if (full)
440 else
442
443 return (*this);
444}
445
448{
451 extract(R);
453 extract(T);
455 RtT = -(R.t() * T);
456
457 Wi.buildFrom(RtT, R.t());
458
459 return Wi;
460}
461
464
467{
468 for (unsigned int i = 0; i < 3; i++)
469 for (unsigned int j = 0; j < 3; j++)
470 R[i][j] = (*this)[i][j];
471}
472
475{
477 extract(R);
478 vpMatrix skTR(3, 3);
479 for (unsigned int i = 0; i < 3; i++)
480 for (unsigned int j = 0; j < 3; j++)
481 skTR[i][j] = (*this)[i][j + 3];
482
483 vpMatrix skT = skTR * R.t();
484 tv[0] = skT[2][1];
485 tv[1] = skT[0][2];
486 tv[2] = skT[1][0];
487}
488
508int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
509{
510 typedef std::string::size_type size_type;
511
512 unsigned int m = getRows();
513 unsigned int n = getCols();
514
515 std::vector<std::string> values(m * n);
516 std::ostringstream oss;
517 std::ostringstream ossFixed;
518 std::ios_base::fmtflags original_flags = oss.flags();
519
520 // ossFixed <<std::fixed;
521 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
522
523 size_type maxBefore = 0; // the length of the integral part
524 size_type maxAfter = 0; // number of decimals plus
525 // one place for the decimal point
526 for (unsigned int i = 0; i < m; ++i) {
527 for (unsigned int j = 0; j < n; ++j) {
528 oss.str("");
529 oss << (*this)[i][j];
530 if (oss.str().find("e") != std::string::npos) {
531 ossFixed.str("");
532 ossFixed << (*this)[i][j];
533 oss.str(ossFixed.str());
534 }
535
536 values[i * n + j] = oss.str();
537 size_type thislen = values[i * n + j].size();
538 size_type p = values[i * n + j].find('.');
539
540 if (p == std::string::npos) {
541 maxBefore = vpMath::maximum(maxBefore, thislen);
542 // maxAfter remains the same
543 } else {
544 maxBefore = vpMath::maximum(maxBefore, p);
545 maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
546 }
547 }
548 }
549
550 size_type totalLength = length;
551 // increase totalLength according to maxBefore
552 totalLength = vpMath::maximum(totalLength, maxBefore);
553 // decrease maxAfter according to totalLength
554 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
555 if (maxAfter == 1)
556 maxAfter = 0;
557
558 // the following line is useful for debugging
559 // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
560
561 if (intro)
562 s << intro;
563 s << "[" << m << "," << n << "]=\n";
564
565 for (unsigned int i = 0; i < m; i++) {
566 s << " ";
567 for (unsigned int j = 0; j < n; j++) {
568 size_type p = values[i * n + j].find('.');
569 s.setf(std::ios::right, std::ios::adjustfield);
570 s.width((std::streamsize)maxBefore);
571 s << values[i * n + j].substr(0, p).c_str();
572
573 if (maxAfter > 0) {
574 s.setf(std::ios::left, std::ios::adjustfield);
575 if (p != std::string::npos) {
576 s.width((std::streamsize)maxAfter);
577 s << values[i * n + j].substr(p, maxAfter).c_str();
578 } else {
579 assert(maxAfter > 1);
580 s.width((std::streamsize)maxAfter);
581 s << ".0";
582 }
583 }
584
585 s << ' ';
586 }
587 s << std::endl;
588 }
589
590 s.flags(original_flags); // restore s to standard state
591
592 return (int)(maxBefore + maxAfter);
593}
594
595#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
596
604
605#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:145
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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()