ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpSimulatorCamera.cpp
1
/****************************************************************************
2
*
3
* $Id: vpSimulatorCamera.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
* Defines the simplest robot : a free flying camera.
36
*
37
* Authors:
38
* Eric Marchand
39
*
40
*****************************************************************************/
41
42
48
#include <visp/vpDebug.h>
49
#include <visp/vpExponentialMap.h>
50
#include <visp/vpHomogeneousMatrix.h>
51
#include <visp/vpRobotException.h>
52
#include <visp/vpSimulatorCamera.h>
53
54
60
vpSimulatorCamera::vpSimulatorCamera
()
61
{
62
init() ;
63
}
64
72
void
vpSimulatorCamera::init()
73
{
74
nDof
= 6;
75
eJe
.
resize
(6,6) ;
76
eJe
.
setIdentity
() ;
77
eJeAvailable
=
true
;
78
fJeAvailable
=
false
;
79
areJointLimitsAvailable
=
false
;
80
qmin
= NULL;
81
qmax
= NULL;
82
83
setMaxTranslationVelocity
(1.);
// vx, vy and vz max set to 1 m/s
84
setMaxRotationVelocity
(
vpMath::rad
(90));
// wx, wy and wz max set to 90 deg/s
85
}
86
87
92
vpSimulatorCamera::~vpSimulatorCamera
()
93
{
94
}
95
106
void
107
vpSimulatorCamera::get_cVe
(
vpVelocityTwistMatrix
&cVe)
108
{
109
vpVelocityTwistMatrix
cVe_;
110
cVe = cVe_;
111
}
112
120
void
121
vpSimulatorCamera::get_eJe
(
vpMatrix
&eJe)
122
{
123
eJe = this->
eJe
;
124
}
125
130
void
131
vpSimulatorCamera::getPosition
(
vpHomogeneousMatrix
&wMc)
const
132
{
133
wMc = this->
wMc_
;
134
}
135
136
/*
137
Get the current position of the robot.
138
139
\param frame : Control frame type in which to get the position, either :
140
- in the camera cartesien frame,
141
- joint (articular) coordinates of each axes
142
- in a reference or fixed cartesien frame attached to the robot base
143
- in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
144
145
\param position : Measured position of the robot:
146
- in camera cartesien frame, a 6 dimension vector, set to 0.
147
148
- in articular, a 6 dimension vector corresponding to the articular
149
position of each dof, first the 3 translations, then the 3
150
articular rotation positions represented by a vpRxyzVector.
151
152
- in reference frame, a 6 dimension vector, the first 3 values correspond to
153
the translation tx, ty, tz in meters (like a vpTranslationVector), and the
154
last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
155
*/
156
void
vpSimulatorCamera::getPosition
(
const
vpRobot::vpControlFrameType
frame,
vpColVector
&q)
157
{
158
q.
resize
(6);
159
160
switch
(frame) {
161
case
vpRobot::CAMERA_FRAME
:
162
q = 0;
163
break
;
164
165
case
vpRobot::ARTICULAR_FRAME
:
166
case
vpRobot::REFERENCE_FRAME
: {
167
// Convert wMc_ to a position
168
// From fMc extract the pose
169
vpRotationMatrix
wRc;
170
this->
wMc_
.
extract
(wRc);
171
vpRxyzVector
rxyz;
172
rxyz.
buildFrom
(wRc);
173
174
for
(
unsigned
int
i=0; i < 3; i++) {
175
q[i] = this->
wMc_
[i][3];
// translation x,y,z
176
q[i+3] = rxyz[i];
// Euler rotation x,y,z
177
}
178
179
break
;
180
}
181
case
vpRobot::MIXT_FRAME
:
182
std::cout <<
"MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()"
<< std::endl;
183
}
184
}
185
214
void
215
vpSimulatorCamera::setVelocity
(
const
vpRobot::vpControlFrameType
frame,
216
const
vpColVector
&v)
217
{
218
if
(
vpRobot::STATE_VELOCITY_CONTROL
!=
getRobotState
()) {
219
setRobotState
(
vpRobot::STATE_VELOCITY_CONTROL
);
220
}
221
222
switch
(frame)
223
{
224
case
vpRobot::ARTICULAR_FRAME
:
225
case
vpRobot::CAMERA_FRAME
: {
226
vpColVector
v_max(6);
227
228
for
(
unsigned
int
i=0; i<3; i++)
229
v_max[i] =
getMaxTranslationVelocity
();
230
for
(
unsigned
int
i=3; i<6; i++)
231
v_max[i] =
getMaxRotationVelocity
();
232
233
vpColVector
v_sat =
vpRobot::saturateVelocities
(v, v_max,
true
);
234
235
wMc_
=
wMc_
*
vpExponentialMap::direct
(v_sat,
delta_t_
);
236
setRobotFrame
(frame);
237
break ;
238
}
239
case
vpRobot::REFERENCE_FRAME
:
240
vpERROR_TRACE
(
"Cannot set a velocity in the reference frame: "
241
"functionality not implemented"
);
242
throw
vpRobotException
(
vpRobotException::wrongStateError
,
243
"Cannot set a velocity in the reference frame:"
244
"functionality not implemented"
);
245
break ;
246
case
vpRobot::MIXT_FRAME
:
247
vpERROR_TRACE
(
"Cannot set a velocity in the mixt frame: "
248
"functionality not implemented"
);
249
throw
vpRobotException
(
vpRobotException::wrongStateError
,
250
"Cannot set a velocity in the mixt frame:"
251
"functionality not implemented"
);
252
253
break ;
254
}
255
}
256
262
void
vpSimulatorCamera::setPosition
(
const
vpHomogeneousMatrix
&wMc)
263
{
264
if
(
vpRobot::STATE_POSITION_CONTROL
!=
getRobotState
()) {
265
setRobotState
(
vpRobot::STATE_POSITION_CONTROL
);
266
}
267
268
this->
wMc_
= wMc;
269
}
src
robot
simulator-robot
vpSimulatorCamera.cpp
Generated on Wed Jun 12 2013 05:39:46 for ViSP by
1.8.1.2