Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpRobot.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Generic virtual robot.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 #include <visp3/robot/vpRobot.h>
39 #include <visp3/robot/vpRobotException.h>
40 #include <visp3/core/vpDebug.h>
41 
42 
44 const double vpRobot::maxRotationVelocityDefault = 0.7;
45 
46 /* ------------------------------------------------------------------------- */
47 /* --- CONSTRUCTEUR -------------------------------------------------------- */
48 /* ------------------------------------------------------------------------- */
49 
51  :
52  stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
53  maxTranslationVelocity (maxTranslationVelocityDefault),
54  maxRotationVelocity (maxRotationVelocityDefault),
55  nDof(0),
56  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
57  qmin(NULL), qmax(NULL), verbose_(true)
58 {
59 }
60 
61 vpRobot::vpRobot (const vpRobot &robot)
62  :
63  stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
64  maxTranslationVelocity (maxTranslationVelocityDefault),
65  maxRotationVelocity (maxRotationVelocityDefault),
66  nDof(0),
67  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
68  qmin(NULL), qmax(NULL), verbose_(true)
69 {
70  *this = robot;
71 }
72 
77 {
78  if (qmin != NULL) {
79  delete [] qmin;
80  qmin = NULL;
81  }
82  if (qmax != NULL) {
83  delete [] qmax;
84  qmax = NULL;
85  }
86 }
87 
90 {
91  stateRobot = robot.stateRobot;
92  frameRobot = robot.frameRobot;
95  nDof = robot.nDof;
96  eJe = robot.eJe;
97  eJeAvailable = robot.eJeAvailable;
98  fJe= robot.fJe;
99  fJeAvailable = robot.fJeAvailable;
101  qmin = new double [nDof];
102  qmax = new double [nDof];
103  for (int i = 0; i< nDof; i++) {
104  qmin[i] = robot.qmin[i];
105  qmax[i] = robot.qmax[i];
106  }
107  verbose_ = robot.verbose_;
108 
109  return (*this);
110 }
163 vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose)
164 {
165  unsigned int size = v_in.size();
166  if (size != v_max.size())
167  throw vpRobotException (vpRobotException::dimensionError, "Velocity vectors should have the same dimension");
168 
169  double scale = 1; // global scale factor to saturate all the axis
170  for (unsigned int i = 0; i < size; i++)
171  {
172  double v_i = fabs(v_in[i]);
173  double v_max_i = fabs(v_max[i]);
174  if ( v_i > v_max_i ) // Test if we should saturate the axis
175  {
176  double scale_i = v_max_i/v_i;
177  if (scale_i < scale)
178  scale = scale_i;
179 
180  if (verbose)
181  std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl;
182  }
183  }
184 
185  vpColVector v_sat(size);
186  v_sat = v_in * scale;
187 
188  return v_sat;
189 }
190 
191 
192 /* -------------------------------------------------------------------------- */
193 /* -------------------------------------------------------------------------- */
194 /* -------------------------------------------------------------------------- */
195 
202 {
203  stateRobot = newState ;
204  return newState ;
205 }
206 
209 {
210  frameRobot = newFrame ;
211  return newFrame ;
212 }
213 
219 {
220  vpColVector r;
221  this ->getPosition (frame, r);
222 
223  return r;
224 }
225 
226 /* ------------------------------------------------------------------------- */
227 /* --- VELOCITY CONTROL ---------------------------------------------------- */
228 /* ------------------------------------------------------------------------- */
229 
237 void
239 {
240  this ->maxTranslationVelocity = v_max;
241  return;
242 }
243 
249 double
251 {
252  return this ->maxTranslationVelocity;
253 }
261 void
263 {
264  this ->maxRotationVelocity = w_max;
265  return;
266 }
267 
274 double
276 {
277  return this ->maxRotationVelocity;
278 }
279 
Error that can be emited by the vpRobot class and its derivates.
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:238
double maxTranslationVelocity
Definition: vpRobot.h:93
double * qmax
Definition: vpRobot.h:111
double maxRotationVelocity
Definition: vpRobot.h:95
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:250
vpRobot(void)
Definition: vpRobot.cpp:50
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:275
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
int eJeAvailable
is the robot Jacobian expressed in the end-effector frame available
Definition: vpRobot.h:103
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:94
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:113
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Get the robot position (frame has to be specified).
static const double maxRotationVelocityDefault
Definition: vpRobot.h:96
int areJointLimitsAvailable
Definition: vpRobot.h:109
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:262
int nDof
number of degrees of freedom
Definition: vpRobot.h:99
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:101
vpRobot & operator=(const vpRobot &robot)
Definition: vpRobot.cpp:89
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
double * qmin
Definition: vpRobot.h:110
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:208
int fJeAvailable
is the robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:107
virtual ~vpRobot()
Definition: vpRobot.cpp:76
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:105