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