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 Franka robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #ifndef _vpRobotFranka_h_
40 #define _vpRobotFranka_h_
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #ifdef VISP_HAVE_FRANKA
45 
46 #include <iostream>
47 #include <thread>
48 #include <atomic>
49 #include <vector>
50 
51 #include <stdio.h>
52 
53 #include <franka/exception.h>
54 #include <franka/robot.h>
55 #include <franka/model.h>
56 #include <franka/gripper.h>
57 
58 #include <visp3/core/vpColVector.h>
59 #include <visp3/robot/vpRobot.h>
60 #include <visp3/core/vpException.h>
61 
62 /*!
63   \class vpRobotFranka
64 
65   \ingroup group_robot_real_arm
66 
67   This class is a wrapper over the [libfranka](https://github.com/frankaemika/libfranka)
68   component part of the [Franka Control Interface](https://frankaemika.github.io/docs/) (FCI).
69 
70   Before using vpRobotFranka follow the
71   [installation instructions](https://frankaemika.github.io/docs/installation.html#) to install
72   libfranka. We suggest to
73   [build libfranka from source](https://frankaemika.github.io/docs/installation.html#building-libfranka)
74   if you are not using ROS.
75 
76   Moreover, you need also to setup a real-time kernel following these
77   [instructions](https://frankaemika.github.io/docs/installation.html#setting-up-the-real-time-kernel).
78 
79   Up to now, this class provides the following capabilities to:
80   - move to a given joint position using setPosition() that is blocking and that returns only when the robot
81     has reached the desired position.
82   \code
83     vpRobotFranka robot("192.168.1.1");
84 
85     vpColVector q_d(7);
86     q_d[3] = -M_PI_2;
87     q_d[5] = M_PI_2;
88     q_d[6] = M_PI_4;
89     std::cout << "Move to joint position: " << q_d.t() << std::endl;
90     robot.setPosition(vpRobot::JOINT_STATE, q_d);
91   \endcode
92   - move applying a joint velocity using setVelocity(). This function is non-blocking.
93   \code
94     vpRobotFranka robot("192.168.1.1");
95 
96     robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
97 
98     vpColVector dq_d(7, 0);
99     dq_d[4] = vpMath::rad(-20.);
100     dq_d[6] = vpMath::rad(20.);
101     while(1) {
102       robot.setVelocity(vpRobot::JOINT_STATE, dq_d);
103       ...
104     }
105   \endcode
106   - move applying a cartesian velocity to the end-effector using setVelocity(). This function is non-blocking.
107   \code
108     vpRobotFranka robot("192.168.1.1");
109 
110     vpColVector ve_d(6);
111     ve_d[2] = 0.02; // vz = 2 cm/s goes down
112 
113     while(1) {
114       robot.setVelocity(vpRobot::END_EFFECTOR_FRAME, ve_d);
115       ...
116     }
117   \endcode
118   - move applying a cartesian velocity to the camera frame (or a given tool frame) using setVelocity().
119     The camera frame (or a tool frame) location wrt the end-effector is set using set_eMc(). This function is non-blocking.
120   \code
121     vpRobotFranka robot("192.168.1.1");
122     vpHomogeneousMatrix eMc; // Position of the camera wrt the end-effector
123     // update eMc
124     robot.set_eMc(eMc);
125 
126     vpColVector vc_d(6);
127     vc_d[2] = 0.02; // vz = 2 cm/s is along the camera optical axis
128 
129     while(1) {
130       robot.setVelocity(vpRobot::CAMERA_FRAME, vc_d);
131       ...
132     }
133   \endcode
134     If the tool attached to the end-effector is not a camera, you can do exactly the same using:
135   \code
136     vpRobotFranka robot("192.168.1.1");
137     vpHomogeneousMatrix eMt;
138     // update eMt, the position of the tool wrt the end-effector frame
139     robot.set_eMc(eMt);
140 
141     vpColVector vt_d(6);
142     vt_d[2] = 0.02; // vt = 2 cm/s is along tool z axis
143 
144     while(1) {
145       robot.setVelocity(vpRobot::TOOL_FRAME, vt_d);
146       ...
147     }
148   \endcode
149 
150   - get the joint position using getPosition()
151   \code
152     vpRobotFranka robot("192.168.1.1");
153 
154     vpColVector q;
155     while(1) {
156       robot.getPosition(vpRobot::JOINT_STATE, q);
157       ...
158     }
159   \endcode
160   - get the cartesian end-effector position using getPosition(). This function is non-blocking.
161   \code
162     vpRobotFranka robot("192.168.1.1");
163 
164     vpPoseVector wPe;
165     vpHomogeneousMatrix wMe;
166     while(1) {
167       robot.getPosition(vpRobot::END_EFFECTOR_FRAME, wPe);
168       wMe.buildFrom(wPe);
169       ...
170     }
171   \endcode
172   - get the cartesian camera (or tool) frame position using getPosition(). This function is non-blocking.
173   \code
174     vpRobotFranka robot("192.168.1.1");
175     vpHomogeneousMatrix eMc;
176     // update eMc, the position of the camera wrt the end-effector frame
177     robot.set_eMc(eMc);
178 
179     vpPoseVector wPc;
180     vpHomogeneousMatrix wMc;
181     while(1) {
182       robot.getPosition(vpRobot::CAMERA_FRAME, wPc);
183       wMc.buildFrom(wPc);
184       ...
185     }
186   \endcode
187     If the tool attached to the end-effector is not a camera, you can do exactly the same using:
188   \code
189     vpRobotFranka robot("192.168.1.1");
190     vpHomogeneousMatrix eMt;
191     // update eMt, the position of the tool wrt the end-effector frame
192     robot.set_eMc(eMt);
193 
194     vpPoseVector wPt;
195     vpHomogeneousMatrix wMt;
196     while(1) {
197       robot.getPosition(vpRobot::TOOL_FRAME, wPt);
198       wMt.buildFrom(wPt);
199       ...
200     }
201   \endcode
202 
203   What is not implemented is:
204   - move to a given cartesian end-effector position
205   - gripper controller
206   - force/torque feadback and control
207 
208   Known issues:
209   - sometimes the joint to joint trajectory generator provided by Franka complains about discontinuities.
210 
211   We provide also the getHandler() function that allows to acces to the robot handler and call the native
212   [libfranka API](https://frankaemika.github.io/libfranka/index.html) fonctionalities:
213   \code
214     vpRobotFranka robot("192.168.1.1");
215 
216     franka::Robot *handler = robot.getHandler();
217 
218     // Get end-effector cartesian position
219     std::array<double, 16> pose = handler->readOnce().O_T_EE;
220   \endcode
221 
222 */
223 class VISP_EXPORT vpRobotFranka : public vpRobot
224 {
225 private:
226   /*!
227     Copy constructor not allowed.
228    */
229   vpRobotFranka(const vpRobotFranka &robot);
230   /*!
231     This function is not implemented.
232    */
getDisplacement(const vpRobot::vpControlFrameType,vpColVector &)233   void getDisplacement(const vpRobot::vpControlFrameType, vpColVector &) {};
234 
235   void init();
236 
237   franka::Robot *m_handler; //!< Robot handler
238   franka::Gripper *m_gripper; //!< Gripper handler
239   franka::Model *m_model;
240   double m_positionningVelocity;
241 
242   // Velocity controller
243   std::thread m_velControlThread;
244   std::atomic_bool m_velControlThreadIsRunning;
245   std::atomic_bool m_velControlThreadStopAsked;
246   std::array<double, 7> m_dq_des;   // Desired joint velocity
247   vpColVector m_v_cart_des;         // Desired cartesian velocity either in reference, end-effector, camera, or tool frame
248 
249   // Force/torque controller
250   std::thread m_ftControlThread;
251   std::atomic_bool m_ftControlThreadIsRunning;
252   std::atomic_bool m_ftControlThreadStopAsked;
253   std::array<double, 7> m_tau_J_des; // Desired joint torques
254   vpColVector m_ft_cart_des;         // Desired cartesian force/torque either in reference, end-effector, camera, or tool frame
255 
256   std::array<double, 7> m_q_min;    // Joint min position
257   std::array<double, 7> m_q_max;    // Joint max position
258   std::array<double, 7> m_dq_max;   // Joint max velocity
259   std::array<double, 7> m_ddq_max;  // Joint max acceleration
260 
261   franka::RobotState m_robot_state; // Robot state protected by mutex
262   std::mutex m_mutex;               // Mutex to protect m_robot_state
263 
264   vpHomogeneousMatrix m_eMc;
265   std::string m_log_folder;
266   std::string m_franka_address;
267 
268 public:
269   vpRobotFranka();
270 
271   vpRobotFranka(const std::string &franka_address,
272                 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
273 
274   virtual ~vpRobotFranka();
275 
276   void connect(const std::string &franka_address,
277                franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
278 
279   vpHomogeneousMatrix get_fMe(const vpColVector &q);
280   vpHomogeneousMatrix get_fMc(const vpColVector &q);
281   vpHomogeneousMatrix get_eMc() const;
282 
283   void get_eJe(vpMatrix &eJe);
284   void get_eJe(const vpColVector &q, vpMatrix &eJe);
285   void get_fJe(vpMatrix &fJe);
286   void get_fJe(const vpColVector &q, vpMatrix &fJe);
287 
288   void getCoriolis(vpColVector &coriolis);
289   void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force);
290 
291   void getGravity(vpColVector &gravity);
292 
293   franka::RobotState getRobotInternalState();
294 
295   /*!
296    * Get gripper handler to access native libfranka functions.
297    *
298    * \return Robot handler if it exists, an exception otherwise.
299    */
getGripperHandler()300   franka::Gripper *getGripperHandler() {
301     if (!m_gripper) {
302       throw(vpException(vpException::fatalError, "Cannot get Franka gripper handler: gripper is not connected"));
303     }
304 
305     return m_gripper;
306   }
307 
308 
309   /*!
310    * Get robot handler to access native libfranka functions.
311    *
312    * \return Robot handler if it exists, an exception otherwise.
313    */
getHandler()314   franka::Robot *getHandler() {
315     if (!m_handler) {
316       throw(vpException(vpException::fatalError, "Cannot get Franka robot handler: robot is not connected"));
317     }
318 
319     return m_handler;
320   }
321 
322   vpColVector getJointMin() const;
323   vpColVector getJointMax() const;
324 
325   void getMass(vpMatrix &mass);
326 
327   void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position);
328   void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &pose);
329 
330   void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position);
331 
332   int gripperClose();
333   int gripperGrasp(double grasping_width, double force=60.);
334   void gripperHoming();
335   int gripperMove(double width);
336   int gripperOpen();
337   void gripperRelease();
338 
339   void move(const std::string &filename, double velocity_percentage=10.);
340 
341   bool readPosFile(const std::string &filename, vpColVector &q);
342   bool savePosFile(const std::string &filename, const vpColVector &q);
343 
344   void set_eMc(const vpHomogeneousMatrix &eMc);
345   void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft,
346                       const double &filter_gain=0.1, const bool &activate_pi_controller=false);
347   void setLogFolder(const std::string &folder);
348   void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position);
349   void setPositioningVelocity(double velocity);
350 
351   vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState);
352   void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel);
353 
354   void stopMotion();
355 };
356 
357 #endif
358 #endif // #ifndef __vpRobotFranka_h_
359