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