Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
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
53 extern "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 
147 int 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 
164 int 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 
185 int 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 
213 int 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 
246 int 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 
277 int 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 
318 int 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 */
340 class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot
341 {
342 
343 public: /* 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 
357 private: /* Not allowed functions. */
361  vpRobotViper850(const vpRobotViper850 &robot);
362 
363 private: /* 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 
394 public: /* 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 
406  void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement);
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 
477 private:
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.
Definition: vpColVector.h:167
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.
Definition: vpPoseVector.h:192
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 get_eJe(vpMatrix &_eJe)=0
Get the robot Jacobian expressed in the end-effector frame.
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 init()=0
virtual void get_fJe(vpMatrix &_fJe)=0
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.
Definition: vpRxyzVector.h:178
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
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1212
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:904
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpViper.cpp:920