ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
testTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * $Id: testTwistMatrix.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Tests some vpMatrix functionalities.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
50 #include <visp/vpConfig.h>
51 #include <visp/vpDebug.h>
52 
53 #include <stdlib.h>
54 #include <stdio.h>
55 
56 #include <visp/vpMath.h>
57 #include <visp/vpMatrix.h>
58 #include <visp/vpColVector.h>
59 #include <visp/vpRxyzVector.h>
60 #include <visp/vpTranslationVector.h>
61 #include <visp/vpRotationMatrix.h>
62 #include <visp/vpVelocityTwistMatrix.h>
63 #include <visp/vpParseArgv.h>
64 
65 // List of allowed command line options
66 #define GETOPTARGS "h"
67 
73 void usage(const char *name, const char *badparam)
74 {
75  fprintf(stdout, "\n\
76 Tests some vpMatrix functionalities.\n\
77 \n\
78 SYNOPSIS\n\
79  %s [-h]\n", name);
80 
81  fprintf(stdout, "\n\
82 OPTIONS: Default\n\
83  -h\n\
84  Print the help.\n");
85 
86  if (badparam)
87  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
88 }
96 bool getOptions(int argc, const char **argv)
97 {
98  const char *optarg;
99  int c;
100  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
101 
102  switch (c) {
103  case 'h': usage(argv[0], NULL); return false; break;
104 
105  default:
106  usage(argv[0], optarg);
107  return false; break;
108  }
109  }
110 
111  if ((c == 1) || (c == -1)) {
112  // standalone param or error
113  usage(argv[0], NULL);
114  std::cerr << "ERROR: " << std::endl;
115  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
116  return false;
117  }
118 
119  return true;
120 }
121 
122 
123 int
124 main(int argc, const char ** argv)
125 {
126  // Read the command line options
127  if (getOptions(argc, argv) == false) {
128  exit (-1);
129  }
130 
131  vpTRACE("--------------------------");
132  vpTRACE("--- TEST vpVelocityTwistMatrix ---");
133  vpTRACE("--------------------------");
134 
135  // Set the translation
137  cte[0] = 1.;
138  cte[1] = 0.5;
139  cte[2] = -1.;
140 
141  // Set the rotation
142  vpRxyzVector cre;
143  cre[0] = M_PI/2.;
144  cre[1] = -M_PI/2.;
145  cre[2] = -M_PI/4.;
146 
147  // Build rotation matrix
148  vpRotationMatrix cRe(cre);
149 
150  // Build the twist matrix
151  vpVelocityTwistMatrix cVe(cte, cRe);
152 
153  vpTRACE("cVe twist matrix:");
154  cVe.print (std::cout, 6);
155 
156 
157  // Set a speed skew
158  vpColVector ev(6);
159 
160  ev[0] = 1.;
161  ev[1] = 0.1;
162  ev[2] = -0.5;
163  ev[3] = M_PI/180.;
164  ev[4] = M_PI/18.;
165  ev[5] = M_PI/10.;
166 
167  vpTRACE("ev colvector:");
168  ev.print (std::cout, 6);
169 
170  // Set a speed skew
171  vpColVector cv;
172 
173  cv = cVe * ev;
174 
175  vpTRACE("cv = cVe * ev:");
176  cv.print (std::cout, 6);
177 
178 }
#define vpTRACE
Definition: vpDebug.h:401
int print(std::ostream &s, unsigned int length, char const *intro=0)
Definition: vpMatrix.cpp:2615
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
The vpRotationMatrix considers the particular case of a rotation matrix.
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
Class that consider the case of a translation vector.