ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpSimulatorPioneerPan.cpp
1
/****************************************************************************
2
*
3
* $Id: vpSimulatorPioneerPan.cpp 2456 2010-01-07 10:33:12Z nmelchio $
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
* Pioneer mobile robot equipped with a pan head simulator without display.
36
*
37
* Authors:
38
* Fabien Spindler
39
*
40
*****************************************************************************/
41
42
49
#include <visp/vpHomogeneousMatrix.h>
50
#include <visp/vpSimulatorPioneerPan.h>
51
#include <visp/vpRobotException.h>
52
#include <visp/vpDebug.h>
53
#include <visp/vpExponentialMap.h>
54
55
65
vpSimulatorPioneerPan::vpSimulatorPioneerPan
()
66
{
67
init() ;
68
}
69
80
void
vpSimulatorPioneerPan::init()
81
{
82
xm_
= 0;
83
ym_
= 0;
84
theta_
= 0;
85
q_pan_
= 0;
86
87
nDof
= 3;
88
eJeAvailable
=
true
;
89
fJeAvailable
=
false
;
90
areJointLimitsAvailable
=
false
;
91
qmin
= NULL;
92
qmax
= NULL;
93
94
wMc_
=
wMm_
*
mMp_
*
pMe_
*
cMe_
.
inverse
();
95
}
96
97
102
vpSimulatorPioneerPan::~vpSimulatorPioneerPan
()
103
{
104
}
105
113
void
114
vpSimulatorPioneerPan::get_eJe
(
vpMatrix
&eJe)
115
{
116
eJe =
vpUnicycle::get_eJe
();
117
}
118
132
void
133
vpSimulatorPioneerPan::setVelocity
(
const
vpRobot::vpControlFrameType
frame,
134
const
vpColVector
&v)
135
{
136
switch
(frame)
137
{
138
case
vpRobot::ARTICULAR_FRAME
:{
139
if
(
vpRobot::STATE_VELOCITY_CONTROL
!=
getRobotState
()) {
140
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
);
141
}
142
143
setRobotFrame
(frame);
144
145
// v is a 3 dimension vector that contains vx, wz, qpan
146
if
(v.
size
() != 3) {
147
vpERROR_TRACE
(
"Bad dimension of the control vector"
);
148
throw
vpRobotException
(
vpRobotException::dimensionError
,
149
"Bad dimension of the control vector"
);
150
}
151
152
vpColVector
v_max(3);
153
154
v_max[0] =
getMaxTranslationVelocity
();
155
v_max[1] =
getMaxRotationVelocity
();
156
v_max[2] =
getMaxRotationVelocity
();
157
158
vpColVector
v_sat =
vpRobot::saturateVelocities
(v, v_max,
true
);
159
160
xm_
+=
delta_t_
* v_sat[0] * cos(
theta_
);
161
ym_
+=
delta_t_
* v_sat[0] * sin(
theta_
);
162
theta_
+=
delta_t_
* v_sat[1];
163
q_pan_
+=
delta_t_
* v_sat[2];
164
165
vpRotationMatrix
wRm(0, 0,
theta_
);
166
vpTranslationVector
wtm(
xm_
,
ym_
, 0);
167
wMm_
.
buildFrom
(wtm, wRm);
168
169
// Update the end effector pose
170
set_pMe
(
q_pan_
);
171
172
// Update the camera pose
173
wMc_
=
wMm_
*
mMp_
*
pMe_
*
cMe_
.
inverse
();
174
175
// Update the jacobian
176
set_eJe
(
q_pan_
);
177
178
break ;
179
}
180
case
vpRobot::CAMERA_FRAME
: {
181
vpERROR_TRACE
(
"Cannot set a velocity in the camera frame: "
182
"functionality not implemented"
);
183
throw
vpRobotException
(
vpRobotException::wrongStateError
,
184
"Cannot set a velocity in the camera frame:"
185
"functionality not implemented"
);
186
break ;
187
}
188
case
vpRobot::REFERENCE_FRAME
:
189
vpERROR_TRACE
(
"Cannot set a velocity in the reference frame: "
190
"functionality not implemented"
);
191
throw
vpRobotException
(
vpRobotException::wrongStateError
,
192
"Cannot set a velocity in the reference frame:"
193
"functionality not implemented"
);
194
break ;
195
196
case
vpRobot::MIXT_FRAME
:
197
vpERROR_TRACE
(
"Cannot set a velocity in the mixt frame: "
198
"functionality not implemented"
);
199
throw
vpRobotException
(
vpRobotException::wrongStateError
,
200
"Cannot set a velocity in the mixt frame:"
201
"functionality not implemented"
);
202
203
break ;
204
}
205
}
206
211
void
212
vpSimulatorPioneerPan::getPosition
(
vpHomogeneousMatrix
&wMc)
const
213
{
214
wMc = this->
wMc_
;
215
}
216
217
/*
218
Get the current position of the robot.
219
220
\param frame : Control frame type in which to get the position, either :
221
- in the camera cartesien frame,
222
- joint (articular) coordinates of each axes (not implemented)
223
- in a reference or fixed cartesien frame attached to the robot base
224
- in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
225
226
\param position : Measured position of the robot:
227
- in camera cartesien frame, a 6 dimension vector, set to 0.
228
229
- in articular, this functionality is not implemented.
230
231
- in reference frame, a 6 dimension vector, the first 3 values correspond to
232
the translation tx, ty, tz in meters (like a vpTranslationVector), and the
233
last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
234
*/
235
void
vpSimulatorPioneerPan::getPosition
(
const
vpRobot::vpControlFrameType
frame,
vpColVector
&q)
236
{
237
q.
resize
(6);
238
239
switch
(frame) {
240
case
vpRobot::CAMERA_FRAME
:
241
q = 0;
242
break
;
243
244
case
vpRobot::ARTICULAR_FRAME
:
245
std::cout <<
"ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()"
<< std::endl;
246
break
;
247
case
vpRobot::REFERENCE_FRAME
: {
248
// Convert wMc_ to a position
249
// From fMc extract the pose
250
vpRotationMatrix
wRc;
251
this->
wMc_
.
extract
(wRc);
252
vpRxyzVector
rxyz;
253
rxyz.
buildFrom
(wRc);
254
255
for
(
unsigned
int
i=0; i < 3; i++) {
256
q[i] = this->
wMc_
[i][3];
// translation x,y,z
257
q[i+3] = rxyz[i];
// Euler rotation x,y,z
258
}
259
260
break
;
261
}
262
case
vpRobot::MIXT_FRAME
:
263
std::cout <<
"MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()"
<< std::endl;
264
}
265
}
src
robot
simulator-robot
vpSimulatorPioneerPan.cpp
Generated on Fri Apr 26 2013 19:54:35 for ViSP by
1.8.1.2