Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpRobot.cpp
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  * Generic virtual robot.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpDebug.h>
40 #include <visp3/robot/vpRobot.h>
41 #include <visp3/robot/vpRobotException.h>
42 
44 const double vpRobot::maxRotationVelocityDefault = 0.7;
45 
46 /* -------------------------------------------------------------------------
47  */
48 /* --- CONSTRUCTEUR --------------------------------------------------------
49  */
50 /* -------------------------------------------------------------------------
51  */
52 
54  : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
55  maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
56  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
57  verbose_(true)
58 {
59 }
60 
62  : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
64  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
65  verbose_(true)
66 {
67  *this = robot;
68 }
69 
74 {
75  if (qmin != NULL) {
76  delete[] qmin;
77  qmin = NULL;
78  }
79  if (qmax != NULL) {
80  delete[] qmax;
81  qmax = NULL;
82  }
83 }
84 
87 {
88  stateRobot = robot.stateRobot;
89  frameRobot = robot.frameRobot;
92  nDof = robot.nDof;
93  eJe = robot.eJe;
94  eJeAvailable = robot.eJeAvailable;
95  fJe = robot.fJe;
96  fJeAvailable = robot.fJeAvailable;
98  qmin = new double[nDof];
99  qmax = new double[nDof];
100  for (int i = 0; i < nDof; i++) {
101  qmin[i] = robot.qmin[i];
102  qmax[i] = robot.qmax[i];
103  }
104  verbose_ = robot.verbose_;
105 
106  return (*this);
107 }
163 vpColVector 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  double v_i = fabs(v_in[i]);
172  double v_max_i = fabs(v_max[i]);
173  if (v_i > v_max_i) // Test if we should saturate the axis
174  {
175  double scale_i = v_max_i / v_i;
176  if (scale_i < scale)
177  scale = scale_i;
178 
179  if (verbose)
180  std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl;
181  }
182  }
183 
184  vpColVector v_sat(size);
185  v_sat = v_in * scale;
186 
187  return v_sat;
188 }
189 
190 /* --------------------------------------------------------------------------
191  */
192 /* --------------------------------------------------------------------------
193  */
194 /* --------------------------------------------------------------------------
195  */
196 
202 {
203  stateRobot = newState;
204  return newState;
205 }
206 
208 {
209  frameRobot = newFrame;
210  return newFrame;
211 }
212 
217 {
218  vpColVector r;
219  this->getPosition(frame, r);
220 
221  return r;
222 }
223 
224 /* -------------------------------------------------------------------------
225  */
226 /* --- VELOCITY CONTROL ----------------------------------------------------
227  */
228 /* -------------------------------------------------------------------------
229  */
230 
239 void vpRobot::setMaxTranslationVelocity(const double v_max)
240 {
241  this->maxTranslationVelocity = v_max;
242  return;
243 }
244 
251 double vpRobot::getMaxTranslationVelocity(void) const { return this->maxTranslationVelocity; }
252 
260 void vpRobot::setMaxRotationVelocity(const double w_max)
261 {
262  this->maxRotationVelocity = w_max;
263  return;
264 }
265 
273 double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; }
Error that can be emited by the vpRobot class and its derivates.
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:239
double maxTranslationVelocity
Definition: vpRobot.h:96
double * qmax
Definition: vpRobot.h:114
double maxRotationVelocity
Definition: vpRobot.h:98
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
vpRobot(void)
Definition: vpRobot.cpp:53
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:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
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:106
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:116
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:99
int areJointLimitsAvailable
Definition: vpRobot.h:112
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:260
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
vpRobot & operator=(const vpRobot &robot)
Definition: vpRobot.cpp:86
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
double * qmin
Definition: vpRobot.h:113
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
int fJeAvailable
is the robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:110
virtual ~vpRobot()
Definition: vpRobot.cpp:73
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108