Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
71vpRobotTemplate::~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 }
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
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
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) {
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:292
Implementation of column vector and the associated operations.
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:152
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
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:100
static const double maxTranslationVelocityDefault
Definition vpRobot.h:95
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ JOINT_STATE
Definition vpRobot.h:78
@ TOOL_FRAME
Definition vpRobot.h:82
@ MIXT_FRAME
Definition vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
static const double maxRotationVelocityDefault
Definition vpRobot.h:97
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
double maxTranslationVelocity
Definition vpRobot.h:94
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
double maxRotationVelocity
Definition vpRobot.h:96