Visual Servoing Platform  version 3.6.1 under development (2024-11-12)
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 
40 BEGIN_VISP_NAMESPACE
42 const double vpRobot::maxRotationVelocityDefault = 0.7;
43 
44 /* -------------------------------------------------------------------------
45  */
46 /* --- CONSTRUCTEUR --------------------------------------------------------
47  */
48 /* -------------------------------------------------------------------------
49  */
50 
52  : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
53  maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
54  eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(nullptr), qmax(nullptr),
55  verbose_(true)
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 }
164 vpColVector vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose)
165 {
166  unsigned int size = v_in.size();
167  if (size != v_max.size())
168  throw vpRobotException(vpRobotException::dimensionError, "Velocity vectors should have the same dimension");
169 
170  double scale = 1; // global scale factor to saturate all the axis
171  for (unsigned int i = 0; i < size; i++) {
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 /* --------------------------------------------------------------------------
196  */
197 
203 {
204  stateRobot = newState;
205  return newState;
206 }
207 
209 {
210  frameRobot = newFrame;
211  return newFrame;
212 }
213 
218 {
219  vpColVector r;
220  this->getPosition(frame, r);
221 
222  return r;
223 }
224 
225 /* -------------------------------------------------------------------------
226  */
227 /* --- VELOCITY CONTROL ----------------------------------------------------
228  */
229 /* -------------------------------------------------------------------------
230  */
231 
241 {
242  this->maxTranslationVelocity = v_max;
243  return;
244 }
245 
252 double vpRobot::getMaxTranslationVelocity(void) const { return this->maxTranslationVelocity; }
253 
262 {
263  this->maxRotationVelocity = w_max;
264  return;
265 }
266 
274 double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; }
275 END_VISP_NAMESPACE
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Error that can be emitted by the vpRobot class and its derivatives.
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
int nDof
number of degrees of freedom
Definition: vpRobot.h:104
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:106
vpRobot & operator=(const vpRobot &robot)
Definition: vpRobot.cpp:83
virtual ~vpRobot()
Definition: vpRobot.cpp:70
double * qmin
Definition: vpRobot.h:115
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:164
vpControlFrameType
Definition: vpRobot.h:77
static const double maxRotationVelocityDefault
Definition: vpRobot.h:101
double * qmax
Definition: vpRobot.h:116
int areJointLimitsAvailable
Definition: vpRobot.h:114
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:112
vpRobotStateType
Definition: vpRobot.h:65
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:99
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:274
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:110
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
double maxTranslationVelocity
Definition: vpRobot.h:98
int eJeAvailable
is the robot Jacobian expressed in the end-effector frame available
Definition: vpRobot.h:108
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252
vpRobot(void)
Definition: vpRobot.cpp:51
double maxRotationVelocity
Definition: vpRobot.h:100
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:261
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:208
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:240
bool verbose_
Definition: vpRobot.h:118