ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpSimulatorPioneer.cpp
1
/****************************************************************************
2
*
3
* $Id: vpSimulatorPioneer.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 simulator without display.
36
*
37
* Authors:
38
* Fabien Spindler
39
*
40
*****************************************************************************/
41
42
49
#include <visp/vpHomogeneousMatrix.h>
50
#include <visp/vpSimulatorPioneer.h>
51
#include <visp/vpRobotException.h>
52
#include <visp/vpDebug.h>
53
#include <visp/vpExponentialMap.h>
54
55
60
vpSimulatorPioneer::vpSimulatorPioneer
()
61
{
62
init() ;
63
}
64
72
void
vpSimulatorPioneer::init()
73
{
74
xm_
= 0;
75
ym_
= 0;
76
theta_
= 0;
77
78
nDof
= 2;
79
eJeAvailable
=
true
;
80
fJeAvailable
=
false
;
81
areJointLimitsAvailable
=
false
;
82
qmin
= NULL;
83
qmax
= NULL;
84
85
wMc_
=
wMe_
*
cMe_
.
inverse
();
86
}
87
88
93
vpSimulatorPioneer::~vpSimulatorPioneer
()
94
{
95
}
96
104
void
105
vpSimulatorPioneer::get_eJe
(
vpMatrix
&eJe)
106
{
107
eJe =
vpUnicycle::get_eJe
();
108
}
109
110
124
void
125
vpSimulatorPioneer::setVelocity
(
const
vpRobot::vpControlFrameType
frame,
126
const
vpColVector
&v)
127
{
128
switch
(frame)
129
{
130
case
vpRobot::ARTICULAR_FRAME
: {
131
if
(
vpRobot::STATE_VELOCITY_CONTROL
!=
getRobotState
()) {
132
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
);
133
}
134
setRobotFrame
(frame);
135
136
// v is a 2 dimension vector that contains v,w
137
if
(v.
size
() != 2) {
138
vpERROR_TRACE
(
"Bad dimension of the control vector"
);
139
throw
vpRobotException
(
vpRobotException::dimensionError
,
140
"Bad dimension of the control vector"
);
141
}
142
143
vpColVector
v_max(2);
144
145
v_max[0] =
getMaxTranslationVelocity
();
146
v_max[1] =
getMaxRotationVelocity
();
147
148
vpColVector
v_sat =
vpRobot::saturateVelocities
(v, v_max,
true
);
149
150
xm_
+=
delta_t_
* v_sat[0] * cos(
theta_
);
151
ym_
+=
delta_t_
* v_sat[0] * sin(
theta_
);
152
theta_
+=
delta_t_
* v_sat[1];
153
154
vpRotationMatrix
wRe(0, 0,
theta_
);
155
vpTranslationVector
wte(
xm_
,
ym_
, 0);
156
wMe_
.
buildFrom
(wte, wRe);
157
wMc_
=
wMe_
*
cMe_
.
inverse
();
158
159
break ;
160
}
161
break ;
162
case
vpRobot::CAMERA_FRAME
:
163
vpERROR_TRACE
(
"Cannot set a velocity in the camera frame: "
164
"functionality not implemented"
);
165
throw
vpRobotException
(
vpRobotException::wrongStateError
,
166
"Cannot set a velocity in the camera frame:"
167
"functionality not implemented"
);
168
break ;
169
case
vpRobot::REFERENCE_FRAME
:
170
vpERROR_TRACE
(
"Cannot set a velocity in the reference frame: "
171
"functionality not implemented"
);
172
throw
vpRobotException
(
vpRobotException::wrongStateError
,
173
"Cannot set a velocity in the articular frame:"
174
"functionality not implemented"
);
175
case
vpRobot::MIXT_FRAME
:
176
vpERROR_TRACE
(
"Cannot set a velocity in the mixt frame: "
177
"functionality not implemented"
);
178
throw
vpRobotException
(
vpRobotException::wrongStateError
,
179
"Cannot set a velocity in the mixt frame:"
180
"functionality not implemented"
);
181
182
break ;
183
}
184
}
185
190
void
191
vpSimulatorPioneer::getPosition
(
vpHomogeneousMatrix
&wMc)
const
192
{
193
wMc = this->
wMc_
;
194
}
195
196
/*
197
Get the current position of the robot.
198
199
\param frame : Control frame type in which to get the position, either :
200
- in the camera cartesien frame,
201
- joint (articular) coordinates of each axes (not implemented)
202
- in a reference or fixed cartesien frame attached to the robot base
203
- in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
204
205
\param position : Measured position of the robot:
206
- in camera cartesien frame, a 6 dimension vector, set to 0.
207
208
- in articular, this functionality is not implemented.
209
210
- in reference frame, a 6 dimension vector, the first 3 values correspond to
211
the translation tx, ty, tz in meters (like a vpTranslationVector), and the
212
last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
213
*/
214
void
vpSimulatorPioneer::getPosition
(
const
vpRobot::vpControlFrameType
frame,
vpColVector
&q)
215
{
216
q.
resize
(6);
217
218
switch
(frame) {
219
case
vpRobot::CAMERA_FRAME
:
220
q = 0;
221
break
;
222
223
case
vpRobot::ARTICULAR_FRAME
:
224
std::cout <<
"ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()"
<< std::endl;
225
break
;
226
case
vpRobot::REFERENCE_FRAME
: {
227
// Convert wMc_ to a position
228
// From fMc extract the pose
229
vpRotationMatrix
wRc;
230
this->
wMc_
.
extract
(wRc);
231
vpRxyzVector
rxyz;
232
rxyz.
buildFrom
(wRc);
233
234
for
(
unsigned
int
i=0; i < 3; i++) {
235
q[i] = this->
wMc_
[i][3];
// translation x,y,z
236
q[i+3] = rxyz[i];
// Euler rotation x,y,z
237
}
238
239
break
;
240
}
241
case
vpRobot::MIXT_FRAME
:
242
std::cout <<
"MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()"
<< std::endl;
243
}
244
}
src
robot
simulator-robot
vpSimulatorPioneer.cpp
Generated on Thu Oct 24 2013 14:47:37 for ViSP by
1.8.4