Visual Servoing Platform version 3.5.0
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 }
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
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
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) {
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}
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
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:102
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
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:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:98