ViSP  2.8.0
vpPioneerPan.h
1 /****************************************************************************
2  *
3  * $Id: vpPioneerPan.h 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  * Common features for Pioneer unicycle mobile robots.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 #ifndef VPPIONEERPAN_H
42 #define VPPIONEERPAN_H
43 
44 #include <visp/vpRxyzVector.h>
45 #include <visp/vpTranslationVector.h>
46 #include <visp/vpHomogeneousMatrix.h>
47 #include <visp/vpUnicycle.h>
48 
49 
99 class VISP_EXPORT vpPioneerPan: public vpUnicycle
100 {
101 public:
106  {
107  double q = 0; // Initial position of the pan axis
108  set_mMp();
109  set_pMe( q );
110  set_cMe();
111  set_eJe( q );
112  }
113 
117  virtual ~vpPioneerPan() {};
118 
144  void set_eJe(double q_pan)
145  {
146  double px = mMp_[0][3];
147  double py = mMp_[1][3];
148  double c1 = cos(q_pan);
149  double s1 = sin(q_pan);
150 
151  eJe_.resize(6,3); // robot jacobian expressed at the pan head end effector
152 
153  eJe_ = 0;
154  eJe_[0][0] = c1;
155  eJe_[0][1] = -c1*py - s1*px;
156 
157  eJe_[2][0] = s1;
158  eJe_[2][1] = -s1*py + c1*px;
159 
160  eJe_[4][1] = -1;
161  eJe_[4][2] = 1;
162  }
163 
164 protected:
168  void set_cMe()
169  {
170  // Position of the camera in the pan frame
171  double cx = 0;
172  double cy = -0.065; // distance between camera and tilt axis
173  double cz = 0;
174  vpTranslationVector etc(cx, cy, cz);
175  vpRotationMatrix eRc;
176  eRc[0][0] = eRc[1][1] = eRc[2][2] = 0;
177  eRc[0][2] = 1;
178  eRc[1][1] = 1;
179  eRc[2][0] = -1;
180 
182  eMc.buildFrom(etc, eRc);
183 
184  cMe_ = eMc.inverse();
185  }
186 
191  void set_mMp()
192  {
193  // Position of the pan head in the mobile platform frame
194  double px = 0.103; // distance between the pan frame and the robot frame
195  double py = 0;
196  double pz = 0.27;
198  mtp.set(px, py, pz);
199 
200  vpRotationMatrix mRp; // set to Identity
201  mRp[1][1] = mRp[2][2] = -1.;
202 
203  mMp_.insert(mtp);
204  mMp_.insert(mRp);
205  }
206 
214  void set_pMe(const double q)
215  {
216  vpRotationMatrix pRe;
217  pRe[0][0] = cos(q);
218  pRe[0][2] = pRe[1][0] = sin(q);
219  pRe[1][1] = pRe[2][2] = 0.;
220  pRe[2][1] = 1.;
221  pRe[1][2] = - pRe[0][0];
222 
223  pMe_.insert(pRe);
224  }
225 
226 protected:
228  vpHomogeneousMatrix pMe_; // depends on q pan
229 
230 };
231 
232 #endif
233 
void set_pMe(const double q)
Definition: vpPioneerPan.h:214
void set_cMe(const vpHomogeneousMatrix &cMe)
Definition: vpUnicycle.h:123
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void set_eJe(const vpMatrix &eJe)
Definition: vpUnicycle.h:134
Generic functions for Pioneer mobile robots equiped with a pan head.
Definition: vpPioneerPan.h:99
The vpRotationMatrix considers the particular case of a rotation matrix.
Generic functions for unicycle mobile robots.
Definition: vpUnicycle.h:60
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:2939
vpHomogeneousMatrix mMp_
Definition: vpPioneerPan.h:227
void set_mMp()
Definition: vpPioneerPan.h:191
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
void set_cMe()
Definition: vpPioneerPan.h:168
vpHomogeneousMatrix pMe_
Definition: vpPioneerPan.h:228
vpHomogeneousMatrix inverse() const
virtual ~vpPioneerPan()
Definition: vpPioneerPan.h:117
void set(const double tx, const double ty, const double tz)
void set_eJe(double q_pan)
Definition: vpPioneerPan.h:144
Class that consider the case of a translation vector.