transf.hh
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2010
4 * Mayte Lázaro, Alejandro R. Mosteo
5 *
6 *
7 * This library is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public
9 * License as published by the Free Software Foundation; either
10 * version 2.1 of the License, or (at your option) any later version.
11 *
12 * This library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with this library; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21
22#ifndef TRANSF_H_
23#define TRANSF_H_
24
25#include <string>
26#include "Eigen/Dense"
27#include "replace/replace.h"
28
29using namespace Eigen;
30using namespace std;
31
32// #define X(t) (t(0,0))
33// #define Y(t) (t(1,0))
34// #define Phi(t) (t(2,0))
35
36class Transf: public MatrixXd {
37
38public:
39 //Constructor and destructor
40 Transf();
41 Transf(double x, double y, double phi);
42 Transf(MatrixXd &m);
43 virtual ~Transf();
44
46 double tX() const;
47 double tY() const;
48 double tPhi() const;
50 double & x() { return (*this)(0, 0); }
51 double & y() { return (*this)(1, 0); }
52 double & phi() { return (*this)(2, 0); }
54 const double x() const { return (*this)(0, 0); }
55 const double y() const { return (*this)(1, 0); }
56 const double phi() const { return (*this)(2, 0); }
57
58 double Distance(const Transf &b) const;
59};
60
61Transf Compose(Transf Tab, Transf Tbc);
62Transf Inv(Transf Tab);
63Transf TRel(Transf Twa, Transf Twb);
64MatrixXd Jacobian (Transf Tab);
65MatrixXd InvJacobian (Transf Tab);
66MatrixXd J1 (Transf Ta, Transf Tb);
67MatrixXd InvJ1 (Transf Ta, Transf Tb);
68MatrixXd J1zero (Transf Ta);
69MatrixXd InvJ1zero (Transf Ta);
70MatrixXd J2 (Transf Ta, Transf Tb);
71MatrixXd InvJ2 (Transf Ta, Transf Tb);
72MatrixXd J2zero (Transf Ta);
73MatrixXd InvJ2zero (Transf Ta);
74double spAtan2 (double y, double x);
75double Normalize (double p);
76
77// Added by Alex
78void Eigenv(MatrixXd M, MatrixXd *vectors, MatrixXd *values);
79 // Compute eigenvalues/vectors.
80 // Results are reset, so the original content/dimensions do not matter.
81 // Results are of same dimensions (NxN) as This, with the caveat that
82 // "values" is a diagonal matrix with the eigenvalues in the diagonal.
83 // This is done to match matlab/octave behavior.
84 // See gsl_eigen_symmv for the guarantees on the results.
85
86#endif /* TRANSF_H_ */
Definition transf.hh:36