Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpRobot.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpDebug.h>
37 #include <visp3/robot/vpRobot.h>
38 #include <visp3/robot/vpRobotException.h>
39 
41 const double vpRobot::maxRotationVelocityDefault = 0.7;
42 
43 /* -------------------------------------------------------------------------
44  */
45 /* --- CONSTRUCTEUR --------------------------------------------------------
46  */
47 /* -------------------------------------------------------------------------
48  */
49 
51  : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
52  maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
53  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(nullptr), qmax(nullptr),
54  verbose_(true)
55 {
56 }
57 
59  : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
60  maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
61  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(nullptr), qmax(nullptr),
62  verbose_(true)
63 {
64  *this = robot;
65 }
66 
71 {
72  if (qmin != nullptr) {
73  delete[] qmin;
74  qmin = nullptr;
75  }
76  if (qmax != nullptr) {
77  delete[] qmax;
78  qmax = nullptr;
79  }
80 }
81 
84 {
85  stateRobot = robot.stateRobot;
86  frameRobot = robot.frameRobot;
89  nDof = robot.nDof;
90  eJe = robot.eJe;
91  eJeAvailable = robot.eJeAvailable;
92  fJe = robot.fJe;
93  fJeAvailable = robot.fJeAvailable;
95  qmin = new double[nDof];
96  qmax = new double[nDof];
97  for (int i = 0; i < nDof; i++) {
98  qmin[i] = robot.qmin[i];
99  qmax[i] = robot.qmax[i];
100  }
101  verbose_ = robot.verbose_;
102 
103  return (*this);
104 }
160 vpColVector vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose)
161 {
162  unsigned int size = v_in.size();
163  if (size != v_max.size())
164  throw vpRobotException(vpRobotException::dimensionError, "Velocity vectors should have the same dimension");
165 
166  double scale = 1; // global scale factor to saturate all the axis
167  for (unsigned int i = 0; i < size; i++) {
168  double v_i = fabs(v_in[i]);
169  double v_max_i = fabs(v_max[i]);
170  if (v_i > v_max_i) // Test if we should saturate the axis
171  {
172  double scale_i = v_max_i / v_i;
173  if (scale_i < scale)
174  scale = scale_i;
175 
176  if (verbose)
177  std::cout << "Excess velocity " << v_in[i] << " axis nr. " << i << std::endl;
178  }
179  }
180 
181  vpColVector v_sat(size);
182  v_sat = v_in * scale;
183 
184  return v_sat;
185 }
186 
187 /* --------------------------------------------------------------------------
188  */
189 /* --------------------------------------------------------------------------
190  */
191 /* --------------------------------------------------------------------------
192  */
193 
199 {
200  stateRobot = newState;
201  return newState;
202 }
203 
205 {
206  frameRobot = newFrame;
207  return newFrame;
208 }
209 
214 {
215  vpColVector r;
216  this->getPosition(frame, r);
217 
218  return r;
219 }
220 
221 /* -------------------------------------------------------------------------
222  */
223 /* --- VELOCITY CONTROL ----------------------------------------------------
224  */
225 /* -------------------------------------------------------------------------
226  */
227 
237 {
238  this->maxTranslationVelocity = v_max;
239  return;
240 }
241 
248 double vpRobot::getMaxTranslationVelocity(void) const { return this->maxTranslationVelocity; }
249 
258 {
259  this->maxRotationVelocity = w_max;
260  return;
261 }
262 
270 double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:339
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
@ dimensionError
Bad dimension.
Definition: vpException.h:83
Error that can be emitted by the vpRobot class and its derivatives.
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
vpRobot & operator=(const vpRobot &robot)
Definition: vpRobot.cpp:83
virtual ~vpRobot()
Definition: vpRobot.cpp:70
double * qmin
Definition: vpRobot.h:113
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
double * qmax
Definition: vpRobot.h:114
int areJointLimitsAvailable
Definition: vpRobot.h:112
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)=0
Get the robot position (frame has to be specified).
int fJeAvailable
is the robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:110
vpRobotStateType
Definition: vpRobot.h:63
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double maxTranslationVelocity
Definition: vpRobot.h:96
int eJeAvailable
is the robot Jacobian expressed in the end-effector frame available
Definition: vpRobot.h:106
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
vpRobot(void)
Definition: vpRobot.cpp:50
double maxRotationVelocity
Definition: vpRobot.h:98
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:257
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:204
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:236
bool verbose_
Definition: vpRobot.h:116