Visual Servoing Platform  version 3.6.1 under development (2023-11-30)
vpRobotTemplate.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  * Defines a robot just to show which function you must implement.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #include <visp3/robot/vpRobotException.h>
39 
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/robot/vpRobotTemplate.h>
47 
52 {
53  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
54  // that is set to identity by default in the constructor.
55 
58 
59  // Set here the robot degrees of freedom number
60  nDof = 6; // If your arm has 6 dof
61 }
62 
67 
71 vpRobotTemplate::~vpRobotTemplate() { std::cout << "Not implemented ! " << std::endl; }
72 
73 /*
74 
75  At least one of these function has to be implemented to control the robot with a
76  Cartesian velocity:
77  - get_eJe()
78  - get_fJe()
79 
80 */
81 
88 {
89  (void)eJe_;
90  std::cout << "Not implemented ! " << std::endl;
91 }
92 
99 {
100  (void)fJe_;
101  std::cout << "Not implemented ! " << std::endl;
102 }
103 
104 /*
105 
106  At least one of these function has to be implemented to control the robot:
107  - setCartVelocity()
108  - setJointVelocity()
109 
110 */
111 
122 {
123  if (v.size() != 6) {
125  "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
126  }
127 
128  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
129  switch (frame) {
130  case vpRobot::TOOL_FRAME: {
131  // We have to transform the requested velocity in the end-effector frame.
132  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
133  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
134  // a velocity twist from tool (or camera) frame into end-effector frame
136  v_e = eVc * v;
137  break;
138  }
139 
142  v_e = v;
143  break;
144  }
146  case vpRobot::MIXT_FRAME:
147  // Out of the scope
148  break;
149  }
150 
151  // Implement your stuff here to send the end-effector velocity twist v_e
152  // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
153  // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
154  // the robot Jacobian in set_fJe() and call:
155  // vpColVector v = get_fJe().inverse() * v_e;
156  // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
157  // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
158  // and call:
159  // vpColVector qdot = get_eJe().inverse() * v_e;
160  // setJointVelocity(qdot);
161  // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
162  // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
163  // velocity in the end-effector into a displacement eMe using the exponetial map:
164  // double delta_t = 0.010; // in sec
165  // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
166  // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
167  // the new position to reach is than given by fMe * eMe
168  // vpColVector fpe(vpPoseVector(fMe * eMe));
169  // setPosition(vpRobot::REFERENCE_FRAME, fpe);
170 
171  std::cout << "Not implemented ! " << std::endl;
172  std::cout << "To implement me you need : " << std::endl;
173  std::cout << "\t to known the robot jacobian expressed in ";
174  std::cout << "the end-effector frame (eJe) " << std::endl;
175  std::cout << "\t the frame transformation between tool or camera frame ";
176  std::cout << "and end-effector frame (cMe)" << std::endl;
177 }
178 
184 {
185  (void)qdot;
186 
187  // Implement your stuff here to send the joint velocities qdot
188 
189  std::cout << "Not implemented ! " << std::endl;
190 }
191 
202 {
205  "Cannot send a velocity to the robot. "
206  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
207  "entering your control loop.");
208  }
209 
210  vpColVector vel_sat(6);
211 
212  // Velocity saturation
213  switch (frame) {
214  // Saturation in cartesian space
215  case vpRobot::TOOL_FRAME:
218  case vpRobot::MIXT_FRAME: {
219  if (vel.size() != 6) {
221  "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
222  }
223  vpColVector vel_max(6);
224 
225  for (unsigned int i = 0; i < 3; i++)
226  vel_max[i] = getMaxTranslationVelocity();
227  for (unsigned int i = 3; i < 6; i++)
228  vel_max[i] = getMaxRotationVelocity();
229 
230  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
231 
232  setCartVelocity(frame, vel_sat);
233  break;
234  }
235  // Saturation in joint space
236  case vpRobot::JOINT_STATE: {
237  if (vel.size() != static_cast<size_t>(nDof)) {
238  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)",
239  nDof, vel.size()));
240  }
241  vpColVector vel_max(vel.size());
242 
243  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
244  vel_max = getMaxRotationVelocity();
245 
246  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
247 
248  setJointVelocity(vel_sat);
249  }
250  }
251 }
252 
253 /*
254 
255  THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
256 
257 */
258 
265 {
266  (void)q;
267  std::cout << "Not implemented ! " << std::endl;
268 }
269 
277 {
278  if (frame == JOINT_STATE) {
279  getJointPosition(q);
280  } else {
281  std::cout << "Not implemented ! " << std::endl;
282  }
283 }
284 
292 {
293  (void)frame;
294  (void)q;
295  std::cout << "Not implemented ! " << std::endl;
296 }
297 
305 {
306  (void)frame;
307  (void)q;
308  std::cout << "Not implemented ! " << std::endl;
309 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:269
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) override
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) override
void getJointPosition(vpColVector &q)
virtual ~vpRobotTemplate() override
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void get_fJe(vpMatrix &fJe_) override
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
void get_eJe(vpMatrix &eJe_) override
void setJointVelocity(const vpColVector &qdot)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override
void init() override
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
double maxRotationVelocity
Definition: vpRobot.h:98