Visual Servoing Platform version 3.5.0
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
245vpForceTwistMatrix::vpForceTwistMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
246 : vpArray2D<double>(6, 6)
247{
248 vpTranslationVector T(tx, ty, tz);
249 vpThetaUVector tu(tux, tuy, tuz);
250 buildFrom(T, tu);
251}
252
272{
274
275 for (unsigned int i = 0; i < 6; i++) {
276 for (unsigned int j = 0; j < 6; j++) {
277 double s = 0;
278 for (unsigned int k = 0; k < 6; k++)
279 s += rowPtrs[i][k] * F.rowPtrs[k][j];
280 Fout[i][j] = s;
281 }
282 }
283 return Fout;
284}
285
294{
295
296 if (6 != M.getRows()) {
298 "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols()));
299 }
300
301 vpMatrix p(6, M.getCols());
302 for (unsigned int i = 0; i < 6; i++) {
303 for (unsigned int j = 0; j < M.getCols(); j++) {
304 double s = 0;
305 for (unsigned int k = 0; k < 6; k++)
306 s += rowPtrs[i][k] * M[k][j];
307 p[i][j] = s;
308 }
309 }
310 return p;
311}
312
358{
359 vpColVector Hout(6);
360
361 if (6 != H.getRows()) {
363 "Cannot multiply a (6x6) force/torque twist matrix by "
364 "a %d dimension column vector",
365 H.getRows()));
366 }
367
368 Hout = 0.0;
369
370 for (unsigned int i = 0; i < 6; i++) {
371 for (unsigned int j = 0; j < 6; j++) {
372 Hout[i] += rowPtrs[i][j] * H[j];
373 }
374 }
375
376 return Hout;
377}
378
399{
400 vpMatrix skewaR = t.skew(t) * R;
401
402 for (unsigned int i = 0; i < 3; i++) {
403 for (unsigned int j = 0; j < 3; j++) {
404 (*this)[i][j] = R[i][j];
405 (*this)[i + 3][j + 3] = R[i][j];
406 (*this)[i + 3][j] = skewaR[i][j];
407 }
408 }
409 return (*this);
410}
411
430{
431 for (unsigned int i = 0; i < 3; i++) {
432 for (unsigned int j = 0; j < 3; j++) {
433 (*this)[i][j] = R[i][j];
434 (*this)[i + 3][j + 3] = R[i][j];
435 (*this)[i + 3][j] = 0;
436 }
437 }
438 return (*this);
439}
440
462{
463 buildFrom(tv, vpRotationMatrix(thetau));
464 return (*this);
465}
466
486{
488 return (*this);
489}
490
520{
521 if (full)
523 else
525
526 return (*this);
527}
528
548int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const *intro) const
549{
550 typedef std::string::size_type size_type;
551
552 unsigned int m = getRows();
553 unsigned int n = getCols();
554
555 std::vector<std::string> values(m * n);
556 std::ostringstream oss;
557 std::ostringstream ossFixed;
558 std::ios_base::fmtflags original_flags = oss.flags();
559
560 // ossFixed <<std::fixed;
561 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
562
563 size_type maxBefore = 0; // the length of the integral part
564 size_type maxAfter = 0; // number of decimals plus
565 // one place for the decimal point
566 for (unsigned int i = 0; i < m; ++i) {
567 for (unsigned int j = 0; j < n; ++j) {
568 oss.str("");
569 oss << (*this)[i][j];
570 if (oss.str().find("e") != std::string::npos) {
571 ossFixed.str("");
572 ossFixed << (*this)[i][j];
573 oss.str(ossFixed.str());
574 }
575
576 values[i * n + j] = oss.str();
577 size_type thislen = values[i * n + j].size();
578 size_type p = values[i * n + j].find('.');
579
580 if (p == std::string::npos) {
581 maxBefore = vpMath::maximum(maxBefore, thislen);
582 // maxAfter remains the same
583 } else {
584 maxBefore = vpMath::maximum(maxBefore, p);
585 maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
586 }
587 }
588 }
589
590 size_type totalLength = length;
591 // increase totalLength according to maxBefore
592 totalLength = vpMath::maximum(totalLength, maxBefore);
593 // decrease maxAfter according to totalLength
594 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
595 if (maxAfter == 1)
596 maxAfter = 0;
597
598 // the following line is useful for debugging
599 // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
600
601 if (intro)
602 s << intro;
603 s << "[" << m << "," << n << "]=\n";
604
605 for (unsigned int i = 0; i < m; i++) {
606 s << " ";
607 for (unsigned int j = 0; j < n; j++) {
608 size_type p = values[i * n + j].find('.');
609 s.setf(std::ios::right, std::ios::adjustfield);
610 s.width((std::streamsize)maxBefore);
611 s << values[i * n + j].substr(0, p).c_str();
612
613 if (maxAfter > 0) {
614 s.setf(std::ios::left, std::ios::adjustfield);
615 if (p != std::string::npos) {
616 s.width((std::streamsize)maxAfter);
617 s << values[i * n + j].substr(p, maxAfter).c_str();
618 } else {
619 assert(maxAfter > 1);
620 s.width((std::streamsize)maxAfter);
621 s << ".0";
622 }
623 }
624
625 s << ' ';
626 }
627 s << std::endl;
628 }
629
630 s.flags(original_flags); // restore s to standard state
631
632 return (int)(maxBefore + maxAfter);
633}
634
635#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
636
644
645#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
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
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=0) 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: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.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.