Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
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
56 extern "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 
150 int 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 
167 int 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 
188 int 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 
216 int 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::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 
249 int 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 
280 int 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 
321 int 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 */
343 class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot
344 {
345 
346 public: /* Constantes */
348  typedef enum {
350  MANUAL,
351  ESTOP
354 
355  /* Vitesse maximale par default lors du positionnement du robot.
356  * C'est la valeur a la construction de l'attribut prive \a
357  * positioningVelocity. Cette valeur peut etre changee par la fonction
358  * #setPositioningVelocity.
359  */
360  static const double defaultPositioningVelocity; // = 20.0;
361 
362 private: /* Not allowed functions. */
366  vpRobotViper850(const vpRobotViper850 &robot);
367 
368 private: /* Attributs prives. */
378  static bool robotAlreadyCreated;
379 
380  double positioningVelocity;
381 
382  // Variables used to compute the measured velocities (see getVelocity() )
383  vpColVector q_prev_getvel;
384  vpHomogeneousMatrix fMc_prev_getvel;
385  double time_prev_getvel;
386  bool first_time_getvel;
387 
388  // Variables used to compute the measured displacement (see
389  // getDisplacement() )
390  vpColVector q_prev_getdis;
391  bool first_time_getdis;
392  vpControlModeType controlMode;
393 
394 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
396 #endif
397 
398 public: /* Methode publiques */
399  explicit vpRobotViper850(bool verbose = true);
400  virtual ~vpRobotViper850(void);
401 
402  // Force/Torque control
403  void biasForceTorqueSensor();
404 
405  void closeGripper() const;
406 
407  void disableJoint6Limits() const;
408  void enableJoint6Limits() const;
409 
410  void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement);
415  vpControlModeType getControlMode() const { return controlMode; }
416 
417  void getForceTorque(vpColVector &H) const;
418  vpColVector getForceTorque() const;
419 
420  double getMaxRotationVelocityJoint6() const;
421  void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position);
422  void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp);
423  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
424  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp);
425 
426  double getPositioningVelocity(void) const;
427  bool getPowerState() const;
428 
429  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity);
430  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp);
431 
432  vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
433  vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double &timestamp);
434 
435  double getTime() const;
436 
437  void get_cMe(vpHomogeneousMatrix &cMe) const;
438  void get_cVe(vpVelocityTwistMatrix &cVe) const;
439  void get_eJe(vpMatrix &eJe);
440  void get_fJe(vpMatrix &fJe);
441 
442  void init(void);
443  void
446  void init(vpViper850::vpToolType tool, const std::string &filename);
447  void init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_);
448 
449  void move(const std::string &filename);
450 
451  void openGripper();
452 
453  void powerOn();
454  void powerOff();
455 
456  static bool readPosFile(const std::string &filename, vpColVector &q);
457  static bool savePosFile(const std::string &filename, const vpColVector &q);
458 
459  void set_eMc(const vpHomogeneousMatrix &eMc_);
460  void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_);
461 
462  void setMaxRotationVelocity(double w_max);
463  void setMaxRotationVelocityJoint6(double w6_max);
464 
465  // Position control
466  void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position);
467  void setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
468  double pos4, double pos5, double pos6);
469  void setPosition(const std::string &filename);
470  void setPositioningVelocity(double velocity);
471 
472  // State
474 
475  // Velocity control
476  void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
477 
478  void stopMotion();
479  void unbiasForceTorqueSensor();
480 
481 private:
482  double maxRotationVelocity_joint6;
483 };
484 
485 #endif
486 #endif /* #ifndef vpRobotViper850_h */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void get_eJe(vpMatrix &_eJe)=0
Get the robot Jacobian expressed in the end-effector frame.
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
vpControlFrameType
Definition: vpRobot.h:75
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
vpRobotStateType
Definition: vpRobot.h:64
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
virtual void init()=0
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Get the robot position (frame has to be specified).
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
virtual void get_fJe(vpMatrix &_fJe)=0
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:103
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static const double defaultPositioningVelocity
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:938
vpControlModeType getControlMode() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
Automatic control mode (default).
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
Class that consider the case of a translation vector.