Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
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(NULL), qmax(NULL),
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(NULL), qmax(NULL),
62 verbose_(true)
63{
64 *this = robot;
65}
66
71{
72 if (qmin != NULL) {
73 delete[] qmin;
74 qmin = NULL;
75 }
76 if (qmax != NULL) {
77 delete[] qmax;
78 qmax = NULL;
79 }
80}
81
84{
85 stateRobot = robot.stateRobot;
86 frameRobot = robot.frameRobot;
89 nDof = robot.nDof;
90 eJe = robot.eJe;
92 fJe = robot.fJe;
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}
160vpColVector 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
249
258{
259 this->maxRotationVelocity = w_max;
260 return;
261}
262
270double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; }
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
@ 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:100
static const double maxTranslationVelocityDefault
Definition vpRobot.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition vpRobot.h:102
vpRobot & operator=(const vpRobot &robot)
Definition vpRobot.cpp:83
virtual ~vpRobot()
Definition vpRobot.cpp:70
double * qmin
Definition vpRobot.h:111
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
static const double maxRotationVelocityDefault
Definition vpRobot.h:97
double * qmax
Definition vpRobot.h:112
int areJointLimitsAvailable
Definition vpRobot.h:110
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:108
vpRobotStateType
Definition vpRobot.h:62
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition vpRobot.h:106
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double maxTranslationVelocity
Definition vpRobot.h:94
int eJeAvailable
is the robot Jacobian expressed in the end-effector frame available
Definition vpRobot.h:104
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
vpRobot(void)
Definition vpRobot.cpp:50
double maxRotationVelocity
Definition vpRobot.h:96
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:114