Visual Servoing Platform  version 3.6.1 under development (2024-06-21)
testTwistMatrix.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Tests some vpMatrix functionalities.
32  */
33 
40 #include <visp3/core/vpConfig.h>
41 #include <visp3/core/vpDebug.h>
42 
43 #include <stdio.h>
44 #include <stdlib.h>
45 
46 #include <visp3/core/vpColVector.h>
47 #include <visp3/core/vpMath.h>
48 #include <visp3/core/vpMatrix.h>
49 #include <visp3/core/vpRotationMatrix.h>
50 #include <visp3/core/vpRxyzVector.h>
51 #include <visp3/core/vpTranslationVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 
54 int main()
55 {
56 #ifdef ENABLE_VISP_NAMESPACE
57  using namespace VISP_NAMESPACE_NAME;
58 #endif
59  try {
60  vpTRACE("--------------------------");
61  vpTRACE("--- TEST vpVelocityTwistMatrix ---");
62  vpTRACE("--------------------------");
63 
64  // Set the translation
66  cte[0] = 1.;
67  cte[1] = 0.5;
68  cte[2] = -1.;
69 
70  // Set the rotation
71  vpRxyzVector cre;
72  cre[0] = M_PI / 2.;
73  cre[1] = -M_PI / 2.;
74  cre[2] = -M_PI / 4.;
75 
76  // Build rotation matrix
77  vpRotationMatrix cRe(cre);
78 
79  // Build the twist matrix
80  vpVelocityTwistMatrix cVe(cte, cRe);
81 
82  vpTRACE("cVe twist matrix:");
83  cVe.print(std::cout, 6);
84 
85  // Set a speed skew
86  vpColVector ev(6);
87 
88  ev[0] = 1.;
89  ev[1] = 0.1;
90  ev[2] = -0.5;
91  ev[3] = M_PI / 180.;
92  ev[4] = M_PI / 18.;
93  ev[5] = M_PI / 10.;
94 
95  vpTRACE("ev colvector:");
96  ev.print(std::cout, 6);
97 
98  // Set a speed skew
99  vpColVector cv;
100 
101  cv = cVe * ev;
102 
103  vpTRACE("cv = cVe * ev:");
104  cv.print(std::cout, 6);
105  return EXIT_SUCCESS;
106  }
107  catch (const vpException &e) {
108  std::cout << "Catch an exception: " << e << std::endl;
109  return EXIT_FAILURE;
110  }
111 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
int print(std::ostream &s, unsigned int length, char const *intro=0) const
error that can be emitted by ViSP classes.
Definition: vpException.h:60
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:179
Class that consider the case of a translation vector.
#define vpTRACE
Definition: vpDebug.h:408