Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpPioneerPan.h
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  * Common features for Pioneer unicycle mobile robots.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 #ifndef VPPIONEERPAN_H
39 #define VPPIONEERPAN_H
40 
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/core/vpRxyzVector.h>
43 #include <visp3/core/vpTranslationVector.h>
44 #include <visp3/robot/vpUnicycle.h>
45 
97 class VISP_EXPORT vpPioneerPan : public vpUnicycle
98 {
99 public:
103  vpPioneerPan() : mMp_(), pMe_()
104  {
105  double q = 0; // Initial position of the pan axis
106  set_mMp();
107  set_pMe(q);
108  set_cMe();
109  set_eJe(q);
110  }
111 
115  virtual ~vpPioneerPan(){};
116 
119 
146  void set_eJe(double q_pan)
147  {
148  double px = mMp_[0][3];
149  double py = mMp_[1][3];
150  double c1 = cos(q_pan);
151  double s1 = sin(q_pan);
152 
153  eJe_.resize(6,
154  3); // robot jacobian expressed at the pan head end effector
155 
156  eJe_ = 0;
157  eJe_[0][0] = c1;
158  eJe_[0][1] = -c1 * py - s1 * px;
159 
160  eJe_[2][0] = s1;
161  eJe_[2][1] = -s1 * py + c1 * px;
162 
163  eJe_[4][1] = -1;
164  eJe_[4][2] = 1;
165  }
167 
168 protected:
175  void set_cMe()
176  {
177  // Position of the camera in the pan frame
178  double cx = 0;
179  double cy = -0.065; // distance between camera and tilt axis
180  double cz = 0;
181  vpTranslationVector etc(cx, cy, cz);
182  vpRotationMatrix eRc;
183  eRc[0][0] = eRc[1][1] = eRc[2][2] = 0;
184  eRc[0][2] = 1;
185  eRc[1][1] = 1;
186  eRc[2][0] = -1;
187 
189  eMc.buildFrom(etc, eRc);
190 
191  cMe_ = eMc.inverse();
192  }
193 
199  void set_mMp()
200  {
201  // Position of the pan head in the mobile platform frame
202  double px = 0.103; // distance between the pan frame and the robot frame
203  double py = 0;
204  double pz = 0.27;
206  mtp.set(px, py, pz);
207 
208  vpRotationMatrix mRp; // set to Identity
209  mRp[1][1] = mRp[2][2] = -1.;
210 
211  mMp_.insert(mtp);
212  mMp_.insert(mRp);
213  }
214 
222  void set_pMe(const double q)
223  {
224  vpRotationMatrix pRe;
225  pRe[0][0] = cos(q);
226  pRe[0][2] = pRe[1][0] = sin(q);
227  pRe[1][1] = pRe[2][2] = 0.;
228  pRe[2][1] = 1.;
229  pRe[1][2] = -pRe[0][0];
230 
231  pMe_.insert(pRe);
232  }
234 
235 protected:
237  vpHomogeneousMatrix pMe_; // depends on q pan
238 };
239 
240 #endif
void set_pMe(const double q)
Definition: vpPioneerPan.h:222
void set_cMe(const vpHomogeneousMatrix &cMe)
Definition: vpUnicycle.h:113
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_eJe(const vpMatrix &eJe)
Definition: vpUnicycle.h:123
Generic functions for Pioneer mobile robots equiped with a pan head.
Definition: vpPioneerPan.h:97
Implementation of a rotation matrix and operations on such kind of matrices.
Generic functions for unicycle mobile robots.
Definition: vpUnicycle.h:56
vpHomogeneousMatrix mMp_
Definition: vpPioneerPan.h:236
void set_mMp()
Definition: vpPioneerPan.h:199
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_cMe()
Definition: vpPioneerPan.h:175
vpHomogeneousMatrix pMe_
Definition: vpPioneerPan.h:237
vpHomogeneousMatrix inverse() const
virtual ~vpPioneerPan()
Definition: vpPioneerPan.h:115
void set(const double tx, const double ty, const double tz)
void set_eJe(double q_pan)
Definition: vpPioneerPan.h:146
Class that consider the case of a translation vector.