Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpRobotTemplate.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  * Defines a robot just to show which function you must implement.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #include <visp3/robot/vpRobotException.h>
42 
48 #include <visp3/core/vpHomogeneousMatrix.h>
49 #include <visp3/robot/vpRobotTemplate.h>
50 
55 {
56  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
57  // that is set to identity by default in the constructor.
58 
61 
62  // Set here the robot degrees of freedom number
63  nDof = 6; // If your arm has 6 dof
64 }
65 
70  : m_eMc()
71 {
72  init();
73 }
74 
79 {
80  std::cout << "Not implemented ! " << std::endl;
81 }
82 
83 /*
84 
85  At least one of these function has to be implemented to control the robot with a
86  Cartesian velocity:
87  - get_eJe()
88  - get_fJe()
89 
90 */
91 
98 {
99  (void) eJe_;
100  std::cout << "Not implemented ! " << std::endl;
101 }
102 
109 {
110  (void) fJe_;
111  std::cout << "Not implemented ! " << std::endl;
112 }
113 
114 /*
115 
116  At least one of these function has to be implemented to control the robot:
117  - setCartVelocity()
118  - setJointVelocity()
119 
120 */
121 
132 {
133  if (v.size() != 6) {
134  throw(vpException(vpException::fatalError, "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
135  }
136 
137  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
138  switch(frame) {
139  case vpRobot::TOOL_FRAME: {
140  // We have to transform the requested velocity in the end-effector frame.
141  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
142  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
143  // a velocity twist from tool (or camera) frame into end-effector frame
145  v_e = eVc * v;
146  break;
147  }
148 
151  v_e = v;
152  break;
153  }
155  case vpRobot::MIXT_FRAME:
156  // Out of the scope
157  break;
158  }
159 
160  // Implement your stuff here to send the end-effector velocity twist v_e
161  // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
162  // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
163  // the robot Jacobian in set_fJe() and call:
164  // vpColVector v = get_fJe().inverse() * v_e;
165  // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
166  // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
167  // and call:
168  // vpColVector qdot = get_eJe().inverse() * v_e;
169  // setJointVelocity(qdot);
170  // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
171  // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
172  // velocity in the end-effector into a displacement eMe using the exponetial map:
173  // double delta_t = 0.010; // in sec
174  // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
175  // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
176  // the new position to reach is than given by fMe * eMe
177  // vpColVector fpe(vpPoseVector(fMe * eMe));
178  // setPosition(vpRobot::REFERENCE_FRAME, fpe);
179 
180  std::cout << "Not implemented ! " << std::endl;
181  std::cout << "To implement me you need : " << std::endl;
182  std::cout << "\t to known the robot jacobian expressed in ";
183  std::cout << "the end-effector frame (eJe) " << std::endl;
184  std::cout << "\t the frame transformation between tool or camera frame ";
185  std::cout << "and end-effector frame (cMe)" << std::endl;
186 }
187 
193 {
194  (void) qdot;
195 
196  // Implement your stuff here to send the joint velocities qdot
197 
198  std::cout << "Not implemented ! " << std::endl;
199 }
200 
211 {
214  "Cannot send a velocity to the robot. "
215  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
216  "entering your control loop.");
217  }
218 
219  vpColVector vel_sat(6);
220 
221  // Velocity saturation
222  switch(frame) {
223  // Saturation in cartesian space
224  case vpRobot::TOOL_FRAME:
227  case vpRobot::MIXT_FRAME: {
228  if (vel.size() != 6) {
229  throw(vpException(vpException::dimensionError, "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
230  }
231  vpColVector vel_max(6);
232 
233  for (unsigned int i = 0; i < 3; i++)
234  vel_max[i] = getMaxTranslationVelocity();
235  for (unsigned int i = 3; i < 6; i++)
236  vel_max[i] = getMaxRotationVelocity();
237 
238  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
239 
240  setCartVelocity(frame, vel_sat);
241  break;
242  }
243  // Saturation in joint space
244  case vpRobot::JOINT_STATE: {
245  if (vel.size() != static_cast<size_t>(nDof)) {
246  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)", nDof, vel.size()));
247  }
248  vpColVector vel_max(vel.size());
249 
250  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
251  vel_max = getMaxRotationVelocity();
252 
253  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
254 
255  setJointVelocity(vel_sat);
256  }
257  }
258 }
259 
260 /*
261 
262  THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
263 
264 */
265 
272 {
273  (void) q;
274  std::cout << "Not implemented ! " << std::endl;
275 }
276 
284 {
285  if (frame == JOINT_STATE) {
286  getJointPosition(q);
287  }
288  else {
289  std::cout << "Not implemented ! " << std::endl;
290  }
291 }
292 
300 {
301  (void) frame;
302  (void) q;
303  std::cout << "Not implemented ! " << std::endl;
304 }
305 
313 {
314  (void) frame;
315  (void) q;
316  std::cout << "Not implemented ! " << std::endl;
317 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Error that can be emited by the vpRobot class and its derivates.
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
double maxTranslationVelocity
Definition: vpRobot.h:96
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:98
virtual ~vpRobotTemplate()
void getJointPosition(vpColVector &q)
error that can be emited by ViSP classes.
Definition: vpException.h:71
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:291
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
void setJointVelocity(const vpColVector &qdot)
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
void get_fJe(vpMatrix &fJe_)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
void get_eJe(vpMatrix &eJe_)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)