Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotViper850.h
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 * Interface for the Irisa's Viper S850 robot controlled by an Adept
33 *MotionBlox.
34 *
35*****************************************************************************/
36
37#ifndef vpRobotViper850_h
38#define vpRobotViper850_h
39
40#include <visp3/core/vpConfig.h>
41
42#ifdef VISP_HAVE_VIPER850
43
44#include <iostream>
45#include <stdio.h>
46
47#include <visp3/core/vpColVector.h>
48#include <visp3/core/vpDebug.h>
49#include <visp3/robot/vpRobot.h>
50#include <visp3/robot/vpViper850.h>
51
52// low level controller api
53extern "C" {
54#include "irisa_Viper850.h"
55#include "trycatch.h"
56}
57
58// If USE_ATI_DAQ defined, use DAQ board instead of serial connexion to
59// acquire data using comedi
60#define USE_ATI_DAQ
61
62#ifdef USE_ATI_DAQ
63#include <visp3/sensor/vpForceTorqueAtiSensor.h>
64#endif
65
66/* !
67 \class vpRobotViper850
68
69 \ingroup group_robot_real_arm
70
71 \brief Control of Irisa's Viper S850 robot named Viper850.
72
73 Implementation of the vpRobot class in order to control Irisa's
74 Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
75 A firewire camera is mounted on the end-effector to allow
76 eye-in-hand visual servoing. The control of this camera is achieved
77 by the vp1394TwoGrabber class.
78
79 The model of the robot is the following:
80 \image html model-viper.png Model of the Viper 850 robot.
81
82 The non modified Denavit-Hartenberg representation of the robot is
83 given in the table below, where \f$q_1^*, \ldots, q_6^*\f$
84 are the variable joint positions.
85
86 \f[
87 \begin{tabular}{|c|c|c|c|c|}
88 \hline
89 Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\
90 \hline
91 1 & $a_1$ & $d_1$ & $-\pi/2$ & $q_1^*$ \\
92 2 & $a_2$ & 0 & 0 & $q_2^*$ \\
93 3 & $a_3$ & 0 & $-\pi/2$ & $q_3^* - \pi$ \\
94 4 & 0 & $d_4$ & $\pi/2$ & $q_4^*$ \\
95 5 & 0 & 0 & $-\pi/2$ & $q_5^*$ \\
96 6 & 0 & 0 & 0 & $q_6^*-\pi$ \\
97 7 & 0 & $d_6$ & 0 & 0 \\
98 \hline
99 \end{tabular}
100 \f]
101
102 In this modelization, different frames have to be considered.
103
104 - \f$ {\cal F}_f \f$: the reference frame, also called world frame
105
106 - \f$ {\cal F}_w \f$: the wrist frame located at the intersection of
107 the last three rotations, with \f$ ^f{\bf M}_w = ^0{\bf M}_6 \f$
108
109 - \f$ {\cal F}_e \f$: the end-effector frame located at the interface of the
110 two tool changers, with \f$^f{\bf M}_e = 0{\bf M}_7 \f$
111
112 - \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
113 M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
114 a calibration stage. We can also consider a custom tool
115 vpViper850::TOOL_CUSTOM and set this during robot initialisation or using
116 set_eMc().
117
118 - \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
119
120 This class allows to control the Viper650 arm robot in position
121 and velocity:
122 - in the joint space (vpRobot::ARTICULAR_FRAME),
123 - in the fixed reference frame \f$ {\cal F}_f \f$ (vpRobot::REFERENCE_FRAME),
124 - in the camera or tool frame \f$ {\cal F}_c \f$ (vpRobot::CAMERA_FRAME),
125 - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
126 in the reference frame \f$ {\cal F}_f \f$ and rotations in the camera or
127 tool frame \f$ {\cal F}_c \f$ .
128
129 End-effector frame (vpRobot::END_EFFECTOR_FRAME) is not implemented.
130
131 All the translations are expressed in meters for positions and m/s
132 for the velocities. Rotations are expressed in radians for the
133 positions, and rad/s for the rotation velocities.
134
135 The direct and inverse kinematics models are implemented in the
136 vpViper850 class.
137
138 \warning A Ctrl-C, a segmentation fault or other system errors are
139 catched by this class to stop the robot.
140
141 To communicate with the robot, you may first create an instance of this
142 class by calling the default constructor:
143
144 \code
145#include <visp3/robot/vpRobotViper850.h>
146
147int main()
148{
149#ifdef VISP_HAVE_VIPER850
150 vpRobotViper850 robot;
151#endif
152}
153 \endcode
154
155 This initialize the robot kinematics with the \f$^e{\bf M}_c\f$
156 extrinsic camera parameters obtained with a projection model without
157 distortion. To set the robot kinematics with the \f$^e{\bf M}_c\f$
158 transformation obtained with a camera perspective model including
159 distortion you need to initialize the robot with:
160
161 \code
162#include <visp3/robot/vpRobotViper850.h>
163
164int main()
165{
166#ifdef VISP_HAVE_VIPER850
167 vpRobotViper850 robot;
168
169 // Set the extrinsic camera parameters obtained with a perpective
170 // projection model including a distortion parameter
171 robot.init(vpViper850::TOOL_MARLIN_F033C_CAMERA, vpCameraParameters::perspectiveProjWithDistortion);
172#endif
173}
174 \endcode
175
176 You can get the intrinsic camera parameters of an image
177 acquired by the camera attached to the robot, with:
178
179 \code
180#include <visp3/core/vpCameraParameters.h>
181#include <visp3/core/vpImage.h>
182#include <visp3/robot/vpRobotViper850.h>
183#include <visp3/sensor/vp1394TwoGrabber.h>
184
185int main()
186{
187#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394)
188 vpImage<unsigned char> I;
189 vp1394TwoGrabber g;
190 g.acquire(I);
191
192 vpRobotViper850 robot;
193
194 // ...
195
196 vpCameraParameters cam;
197 robot.getCameraParameters(cam, I);
198 // In cam, you get the intrinsic parameters of the projection model
199 // with distortion.
200#endif
201}
202 \endcode
203
204 To control the robot in position, you may set the controller
205 to position control and than send the position to reach in a specific
206 frame like here in the joint space:
207
208 \code
209#include <visp3/core/vpColVector.h>
210#include <visp3/core/vpMath.h>
211#include <visp3/robot/vpRobotViper850.h>
212
213int main()
214{
215#ifdef VISP_HAVE_VIPER850
216 vpRobotViper850 robot;
217
218 vpColVector q(6);
219 // Set a joint position
220 q[0] = vpMath::rad(10); // Joint 1 position, in rad
221 q[1] = 0.2; // Joint 2 position, in rad
222 q[2] = 0.3; // Joint 3 position, in rad
223 q[3] = M_PI/8; // Joint 4 position, in rad
224 q[4] = M_PI/4; // Joint 5 position, in rad
225 q[5] = M_PI; // Joint 6 position, in rad
226
227 // Initialize the controller to position control
228 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
229
230 // Moves the robot in the joint space
231 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
232#endif
233}
234 \endcode
235
236 The robot moves to the specified position with the default
237 positioning velocity vpRobotViper850::m_defaultPositioningVelocity. The
238 setPositioningVelocity() method allows to change the maximal
239 velocity used to reach the desired position.
240
241 \code
242#include <visp3/core/vpColVector.h>
243#include <visp3/core/vpMath.h>
244#include <visp3/robot/vpRobotViper850.h>
245
246int main()
247{
248#ifdef VISP_HAVE_VIPER850
249 vpRobotViper850 robot;
250
251 vpColVector q(6);
252 // Set q[i] with i in [0:5]
253
254 // Initialize the controller to position control
255 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
256
257 // Set the max velocity to 40%
258 robot.setPositioningVelocity(40);
259
260 // Moves the robot in the joint space
261 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
262#endif
263}
264 \endcode
265
266 To control the robot in velocity, you may set the controller to
267 velocity control and than send the velocities. To end the velocity
268 control and stop the robot you have to set the controller to the
269 stop state. Here is an example of a velocity control in the joint
270 space:
271
272 \code
273#include <visp3/core/vpColVector.h>
274#include <visp3/core/vpMath.h>
275#include <visp3/robot/vpRobotViper850.h>
276
277int main()
278{
279#ifdef VISP_HAVE_VIPER850
280 vpRobotViper850 robot;
281
282 vpColVector qvel(6);
283 // Set a joint velocity
284 qvel[0] = 0.1; // Joint 1 velocity in rad/s
285 qvel[1] = vpMath::rad(15); // Joint 2 velocity in rad/s
286 qvel[2] = 0; // Joint 3 velocity in rad/s
287 qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
288 qvel[4] = 0; // Joint 5 velocity in rad/s
289 qvel[5] = 0; // Joint 6 velocity in rad/s
290
291 // Initialize the controller to position control
292 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
293
294 for ( ; ; ) {
295 // Apply a velocity in the joint space
296 robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
297
298 // Compute new velocities qvel...
299 }
300
301 // Stop the robot
302 robot.setRobotState(vpRobot::STATE_STOP);
303#endif
304}
305 \endcode
306
307 It is also possible to specify the position of a custom tool cartesian
308 frame. To this end this frame is to specify with respect of the end effector
309 frame in \f$^e {\bf M}_c\f$ transformation. This could be done by initializing
310 the robot thanks to init(vpViper850::vpToolType, const vpHomogeneousMatrix &)
311 or init(vpViper850::vpToolType, const std::string &) or using set_eMc(). The
312 following example illustrates this usecase:
313
314 \code
315#include <visp3/core/vpHomogeneousMatrix.h>
316#include <visp3/robot/vpRobotViper850.h>
317
318int main()
319{
320#ifdef VISP_HAVE_VIPER850
321 vpRobotViper850 robot;
322
323 // Set the transformation between the end-effector frame
324 // and the tool frame.
325 vpHomogeneousMatrix eMc(0.001, 0.0, 0.1, 0.0, 0.0, M_PI/2);
326
327 robot.init(vpViper850::TOOL_CUSTOM, eMc);
328#endif
329}
330 \endcode
331
332 It is also possible to measure the robot current position with
333 getPosition() method and the robot current velocities with the getVelocity()
334 method.
335
336 For convenience, there is also the ability to read/write joint
337 positions from a position file with readPosFile() and savePosFile()
338 methods.
339*/
340class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot
341{
342
343public: /* Constantes */
345 typedef enum {
347 MANUAL,
349 ESTOP
350 } vpControlModeType;
351
352 /* Max velocity used during robot control in position.
353 * this value could be changed using setPositioningVelocity().
354 */
355 static const double m_defaultPositioningVelocity; // = 20.0;
356
357private: /* Not allowed functions. */
361 vpRobotViper850(const vpRobotViper850 &robot);
362
363private: /* Attributs prives. */
373 static bool m_robotAlreadyCreated;
374
375 double m_positioningVelocity;
376
377 // Variables used to compute the measured velocities (see getVelocity() )
378 vpColVector m_q_prev_getvel;
379 vpHomogeneousMatrix m_fMc_prev_getvel;
380 vpHomogeneousMatrix m_fMe_prev_getvel;
381 double m_time_prev_getvel;
382 bool m_first_time_getvel;
383
384 // Variables used to compute the measured displacement (see
385 // getDisplacement() )
386 vpColVector m_q_prev_getdis;
387 bool m_first_time_getdis;
388 vpControlModeType m_controlMode;
389
390#if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
392#endif
393
394public: /* Methode publiques */
395 explicit vpRobotViper850(bool verbose = true);
396 virtual ~vpRobotViper850(void);
397
398 // Force/Torque control
399 void biasForceTorqueSensor();
400
401 void closeGripper() const;
402
403 void disableJoint6Limits() const;
404 void enableJoint6Limits() const;
405
411 vpControlModeType getControlMode() const { return m_controlMode; }
412
413 void getForceTorque(vpColVector &H) const;
414 vpColVector getForceTorque() const;
415
416 double getMaxRotationVelocityJoint6() const;
417 void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position);
418 void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp);
419 void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
420 void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp);
421
422 double getPositioningVelocity(void) const;
423 bool getPowerState() const;
424
425 void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity);
426 void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp);
427
428 vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
429 vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double &timestamp);
430
431 double getTime() const;
432
433 void get_cMe(vpHomogeneousMatrix &cMe) const;
434 void get_cVe(vpVelocityTwistMatrix &cVe) const;
435 void get_eJe(vpMatrix &eJe);
436 void get_fJe(vpMatrix &fJe);
437
438 void init(void);
439 void
442 void init(vpViper850::vpToolType tool, const std::string &filename);
443 void init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_);
444
445 void move(const std::string &filename);
446
447 void openGripper();
448
449 void powerOn();
450 void powerOff();
451
452 static bool readPosFile(const std::string &filename, vpColVector &q);
453 static bool savePosFile(const std::string &filename, const vpColVector &q);
454
455 void set_eMc(const vpHomogeneousMatrix &eMc_);
456 void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_);
457
458 void setMaxRotationVelocity(double w_max);
459 void setMaxRotationVelocityJoint6(double w6_max);
460
461 // Position control
462 void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position);
463 void setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3, double pos4,
464 double pos5, double pos6);
465 void setPosition(const std::string &filename);
466 void setPositioningVelocity(double velocity);
467
468 // State
470
471 // Velocity control
472 void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
473
474 void stopMotion();
475 void unbiasForceTorqueSensor();
476
477private:
478 double maxRotationVelocity_joint6;
479};
480
481#endif
482#endif /* #ifndef vpRobotViper850_h */
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of a pose vector and operations on poses.
static const double m_defaultPositioningVelocity
@ AUTO
Automatic control mode (default).
vpControlModeType getControlMode() const
Class that defines a generic virtual robot.
Definition vpRobot.h:57
vpControlFrameType
Definition vpRobot.h:73
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)=0
Get the robot position (frame has to be specified).
vpRobotStateType
Definition vpRobot.h:62
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
void setMaxRotationVelocity(double maxVr)
Definition vpRobot.cpp:257
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.
Modelisation of the ADEPT Viper 850 robot.
Definition vpViper850.h:101
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper850.h:125
void init(void)
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition vpViper.cpp:1212
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpViper.cpp:904
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpViper.cpp:1141
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpViper.cpp:952
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition vpViper.cpp:920