Visual Servoing Platform  version 3.5.1 under development (2022-12-01)
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 
74 vpRobotTemplate::~vpRobotTemplate() { std::cout << "Not implemented ! " << std::endl; }
75 
76 /*
77 
78  At least one of these function has to be implemented to control the robot with a
79  Cartesian velocity:
80  - get_eJe()
81  - get_fJe()
82 
83 */
84 
91 {
92  (void)eJe_;
93  std::cout << "Not implemented ! " << std::endl;
94 }
95 
102 {
103  (void)fJe_;
104  std::cout << "Not implemented ! " << std::endl;
105 }
106 
107 /*
108 
109  At least one of these function has to be implemented to control the robot:
110  - setCartVelocity()
111  - setJointVelocity()
112 
113 */
114 
125 {
126  if (v.size() != 6) {
128  "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
129  }
130 
131  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
132  switch (frame) {
133  case vpRobot::TOOL_FRAME: {
134  // We have to transform the requested velocity in the end-effector frame.
135  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
136  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
137  // a velocity twist from tool (or camera) frame into end-effector frame
139  v_e = eVc * v;
140  break;
141  }
142 
145  v_e = v;
146  break;
147  }
149  case vpRobot::MIXT_FRAME:
150  // Out of the scope
151  break;
152  }
153 
154  // Implement your stuff here to send the end-effector velocity twist v_e
155  // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
156  // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
157  // the robot Jacobian in set_fJe() and call:
158  // vpColVector v = get_fJe().inverse() * v_e;
159  // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
160  // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
161  // and call:
162  // vpColVector qdot = get_eJe().inverse() * v_e;
163  // setJointVelocity(qdot);
164  // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
165  // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
166  // velocity in the end-effector into a displacement eMe using the exponetial map:
167  // double delta_t = 0.010; // in sec
168  // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
169  // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
170  // the new position to reach is than given by fMe * eMe
171  // vpColVector fpe(vpPoseVector(fMe * eMe));
172  // setPosition(vpRobot::REFERENCE_FRAME, fpe);
173 
174  std::cout << "Not implemented ! " << std::endl;
175  std::cout << "To implement me you need : " << std::endl;
176  std::cout << "\t to known the robot jacobian expressed in ";
177  std::cout << "the end-effector frame (eJe) " << std::endl;
178  std::cout << "\t the frame transformation between tool or camera frame ";
179  std::cout << "and end-effector frame (cMe)" << std::endl;
180 }
181 
187 {
188  (void)qdot;
189 
190  // Implement your stuff here to send the joint velocities qdot
191 
192  std::cout << "Not implemented ! " << std::endl;
193 }
194 
205 {
208  "Cannot send a velocity to the robot. "
209  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
210  "entering your control loop.");
211  }
212 
213  vpColVector vel_sat(6);
214 
215  // Velocity saturation
216  switch (frame) {
217  // Saturation in cartesian space
218  case vpRobot::TOOL_FRAME:
221  case vpRobot::MIXT_FRAME: {
222  if (vel.size() != 6) {
224  "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
225  }
226  vpColVector vel_max(6);
227 
228  for (unsigned int i = 0; i < 3; i++)
229  vel_max[i] = getMaxTranslationVelocity();
230  for (unsigned int i = 3; i < 6; i++)
231  vel_max[i] = getMaxRotationVelocity();
232 
233  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
234 
235  setCartVelocity(frame, vel_sat);
236  break;
237  }
238  // Saturation in joint space
239  case vpRobot::JOINT_STATE: {
240  if (vel.size() != static_cast<size_t>(nDof)) {
241  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)",
242  nDof, vel.size()));
243  }
244  vpColVector vel_max(vel.size());
245 
246  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
247  vel_max = getMaxRotationVelocity();
248 
249  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
250 
251  setJointVelocity(vel_sat);
252  }
253  }
254 }
255 
256 /*
257 
258  THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
259 
260 */
261 
268 {
269  (void)q;
270  std::cout << "Not implemented ! " << std::endl;
271 }
272 
280 {
281  if (frame == JOINT_STATE) {
282  getJointPosition(q);
283  } else {
284  std::cout << "Not implemented ! " << std::endl;
285  }
286 }
287 
295 {
296  (void)frame;
297  (void)q;
298  std::cout << "Not implemented ! " << std::endl;
299 }
300 
308 {
309  (void)frame;
310  (void)q;
311  std::cout << "Not implemented ! " << std::endl;
312 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Error that can be emited by the vpRobot class and its derivates.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_eJe(vpMatrix &eJe_)
void get_fJe(vpMatrix &fJe_)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getJointPosition(vpColVector &q)
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
virtual ~vpRobotTemplate()
void setJointVelocity(const vpColVector &qdot)
int nDof
number of degrees of freedom
Definition: vpRobot.h:103
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:98
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ JOINT_STATE
Definition: vpRobot.h:81
@ TOOL_FRAME
Definition: vpRobot.h:85
@ MIXT_FRAME
Definition: vpRobot.h:87
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
static const double maxRotationVelocityDefault
Definition: vpRobot.h:100
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
double maxTranslationVelocity
Definition: vpRobot.h:97
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:99