Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
173
186
202vpVelocityTwistMatrix::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
396
410{
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
505int 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 } else {
541 maxBefore = vpMath::maximum(maxBefore, p);
542 maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
543 }
544 }
545 }
546
547 size_type totalLength = length;
548 // increase totalLength according to maxBefore
549 totalLength = vpMath::maximum(totalLength, maxBefore);
550 // decrease maxAfter according to totalLength
551 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
552 if (maxAfter == 1)
553 maxAfter = 0;
554
555 // the following line is useful for debugging
556 // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
557
558 if (intro)
559 s << intro;
560 s << "[" << m << "," << n << "]=\n";
561
562 for (unsigned int i = 0; i < m; i++) {
563 s << " ";
564 for (unsigned int j = 0; j < n; j++) {
565 size_type p = values[i * n + j].find('.');
566 s.setf(std::ios::right, std::ios::adjustfield);
567 s.width((std::streamsize)maxBefore);
568 s << values[i * n + j].substr(0, p).c_str();
569
570 if (maxAfter > 0) {
571 s.setf(std::ios::left, std::ios::adjustfield);
572 if (p != std::string::npos) {
573 s.width((std::streamsize)maxAfter);
574 s << values[i * n + j].substr(p, maxAfter).c_str();
575 } else {
576 assert(maxAfter > 1);
577 s.width((std::streamsize)maxAfter);
578 s << ".0";
579 }
580 }
581
582 s << ' ';
583 }
584 s << std::endl;
585 }
586
587 s.flags(original_flags); // restore s to standard state
588
589 return (int)(maxBefore + maxAfter);
590}
591
592#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
593
601
602#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
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:280
double ** rowPtrs
Address of the first element of each rows.
Definition vpArray2D.h:138
vpArray2D< double > t() const
Compute the transpose of the array.
Definition vpArray2D.h:1059
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
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:172
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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()