Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpRobotFranka.cpp
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 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_FRANKA
42 
43 #include <visp3/robot/vpRobotException.h>
44 #include <visp3/robot/vpRobotFranka.h>
45 #include <visp3/core/vpIoTools.h>
46 
47 #include "vpJointPosTrajGenerator_impl.h"
48 #include "vpJointVelTrajGenerator_impl.h"
49 
56  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_controlThread(), m_controlThreadIsRunning(false),
57  m_controlThreadStopAsked(false), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
58  m_mutex(), m_dq_des(), m_eMc(), m_log_folder(), m_franka_address()
59 {
60  init();
61 }
62 
69 vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
70  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_controlThread(), m_controlThreadIsRunning(false),
71  m_controlThreadStopAsked(false), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
72  m_mutex(), m_dq_des(), m_v_cart_des(), m_eMc(),m_log_folder(), m_franka_address()
73 {
74  init();
75  connect(franka_address, realtime_config);
76 }
77 
81 void vpRobotFranka::init()
82 {
83  nDof = 7;
84 
85  m_q_min = std::array<double, 7> {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
86  m_q_max = std::array<double, 7> {12.8973, 1.7628, 2.8973, 0.0175, 2.8973, 3.7525, 2.8973};
87  m_dq_max = std::array<double, 7> {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
88  m_ddq_max = std::array<double, 7> {14.25, 7.125, 11.875, 11.875, 14.25, 19.0, 19.0};
89 }
90 
97 {
99 
100  if (m_handler)
101  delete m_handler;
102 
103  if (m_gripper) {
104  std::cout << "Grasped object, will release it now." << std::endl;
105  m_gripper->stop();
106  delete m_gripper;
107  }
108 
109  if (m_model) {
110  delete m_model;
111  }
112 }
113 
120 void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
121 {
122  init();
123  if(franka_address.empty()) {
124  throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
125  }
126  if (m_handler)
127  delete m_handler;
128 
129  m_franka_address = franka_address;
130  m_handler = new franka::Robot(franka_address, realtime_config);
131 
132  std::array<double, 7> lower_torque_thresholds_nominal{
133  {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
134  std::array<double, 7> upper_torque_thresholds_nominal{
135  {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
136  std::array<double, 7> lower_torque_thresholds_acceleration{
137  {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
138  std::array<double, 7> upper_torque_thresholds_acceleration{
139  {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
140  std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
141  std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
142  std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
143  std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
144  m_handler->setCollisionBehavior(
145  lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
146  lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
147  lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
148  lower_force_thresholds_nominal, upper_force_thresholds_nominal);
149 
150  m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
151  m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
152 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
153  // m_handler->setFilters(100, 100, 100, 100, 100);
154  m_handler->setFilters(10, 10, 10, 10, 10);
155 #else
156  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
157 #endif
158  if (m_model) {
159  delete m_model;
160  }
161  m_model = new franka::Model(m_handler->loadModel());
162 }
163 
179 {
180  if (!m_handler) {
181  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
182  }
183 
184  franka::RobotState robot_state = getRobotInternalState();
185  vpColVector q(7);
186  for (int i=0; i < 7; i++)
187  q[i] = robot_state.q_d[i];
188 
189  switch(frame) {
190  case JOINT_STATE: {
191  position = q;
192  break;
193  }
194  case END_EFFECTOR_FRAME: {
195  position.resize(6);
196  vpHomogeneousMatrix fMe = get_fMe(q);
197  vpPoseVector fPe(fMe);
198  for (size_t i=0; i < 6; i++) {
199  position[i] = fPe[i];
200  }
201  break;
202  }
203  case TOOL_FRAME: { // same a CAMERA_FRAME
204  position.resize(6);
205  vpHomogeneousMatrix fMc = get_fMc(q);
206  vpPoseVector fPc(fMc);
207  for (size_t i=0; i < 6; i++) {
208  position[i] = fPc[i];
209  }
210  break;
211  }
212  default: {
213  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
214  }
215  }
216 }
217 
232 {
233  if (!m_handler) {
234  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
235  }
236 
237  franka::RobotState robot_state = getRobotInternalState();
238 
239  switch(frame) {
240  case JOINT_STATE: {
241  force.resize(7);
242  for (int i=0; i < 7; i++)
243  force[i] = robot_state.tau_J[i];
244 
245  break;
246  }
247  case END_EFFECTOR_FRAME: {
248  force.resize(6);
249  for (int i=0; i < 7; i++)
250  force[i] = robot_state.K_F_ext_hat_K[i];
251  break;
252  }
253  case TOOL_FRAME: {
254  // end-effector frame
255  vpColVector eFe(6);
256  for (int i=0; i < 7; i++)
257  eFe[i] = robot_state.K_F_ext_hat_K[i];
258 
259  // Transform in tool frame
261  vpForceTwistMatrix cWe( cMe );
262  force = cWe * eFe;
263  break;
264  }
265  default: {
266  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
267  }
268  }
269 }
270 
286 {
287  if (!m_handler) {
288  throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
289  }
290 
291  franka::RobotState robot_state = getRobotInternalState();
292 
293  switch(frame) {
294 
295  case JOINT_STATE: {
296  d_position.resize(7);
297  for (int i=0; i < 7; i++) {
298  d_position[i]=robot_state.dq[i];
299  }
300  break;
301  }
302 
303  default: {
304  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
305  }
306  }
307 }
308 
315 {
316  if (!m_handler) {
317  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
318  }
319 
320  std::array<double, 7> coriolis_;
321 
322  franka::RobotState robot_state = getRobotInternalState();
323 
324  coriolis_ = m_model->coriolis(robot_state);
325 
326  coriolis.resize(7);
327  for (int i=0; i < 7; i++) {
328  coriolis[i] = coriolis_[i];
329  }
330 }
331 
337 {
338  if (!m_handler) {
339  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
340  }
341 
342  std::array<double, 7> gravity_;
343 
344  franka::RobotState robot_state = getRobotInternalState();
345 
346  gravity_ = m_model->gravity(robot_state);
347 
348  gravity.resize(7);
349  for (int i=0; i < 7; i++) {
350  gravity[i] = gravity_[i];
351  }
352 }
353 
359 {
360  if (!m_handler) {
361  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
362  }
363 
364  std::array<double, 49> mass_;
365 
366  franka::RobotState robot_state = getRobotInternalState();
367 
368  mass_ = m_model->mass(robot_state); // column-major
369 
370  mass.resize(7, 7); // row-major
371  for (size_t i = 0; i < 7; i ++) {
372  for (size_t j = 0; j < 7; j ++) {
373  mass[i][j] = mass_[j*7 + i];
374  }
375  }
376 }
377 
386 {
387  if (!m_handler) {
388  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
389  }
390  if (q.size() != 7) {
391  throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
392  }
393 
394  std::array< double, 7 > q_array;
395  for (size_t i = 0; i < 7; i++)
396  q_array[i] = q[i];
397 
398  franka::RobotState robot_state = getRobotInternalState();
399 
400  std::array<double, 16> pose_array = m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
402  for (unsigned int i=0; i< 4; i++) {
403  for (unsigned int j=0; j< 4; j++) {
404  fMe[i][j] = pose_array[j*4 + i];
405  }
406  }
407 
408  return fMe;
409 }
410 
426 {
427  vpHomogeneousMatrix fMe = get_fMe(q);
428  return (fMe * m_eMc);
429 }
430 
442 {
443  if (!m_handler) {
444  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
445  }
446 
447  franka::RobotState robot_state = getRobotInternalState();
448 
449  std::array<double, 16> pose_array = robot_state.O_T_EE;
451  for (unsigned int i=0; i< 4; i++) {
452  for (unsigned int j=0; j< 4; j++) {
453  fMe[i][j] = pose_array[j*4 + i];
454  }
455  }
456 
457  switch(frame) {
458  case END_EFFECTOR_FRAME: {
459  pose.buildFrom(fMe);
460  break;
461  }
462  case TOOL_FRAME: {
463  pose.buildFrom(fMe * m_eMc);
464  break;
465  }
466  default: {
467  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
468  }
469  }
470 }
471 
478 {
479  if (!m_handler) {
480  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
481  }
482 
483  franka::RobotState robot_state = getRobotInternalState();
484 
485  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
486  eJe.resize(6, 7); // row-major
487  for (size_t i = 0; i < 6; i ++) {
488  for (size_t j = 0; j < 7; j ++) {
489  eJe[i][j] = jacobian[j*6 + i];
490  }
491  }
492  // TODO check from vpRobot fJe and fJeAvailable
493 
494 }
495 
503 {
504  if (!m_handler) {
505  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
506  }
507 
508  franka::RobotState robot_state = getRobotInternalState();
509 
510  std::array< double, 7 > q_array;
511  for (size_t i = 0; i < 7; i++)
512  q_array[i] = q[i];
513 
514  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
515  eJe.resize(6, 7); // row-major
516  for (size_t i = 0; i < 6; i ++) {
517  for (size_t j = 0; j < 7; j ++) {
518  eJe[i][j] = jacobian[j*6 + i];
519  }
520  }
521  // TODO check from vpRobot fJe and fJeAvailable
522 
523 }
524 
531 {
532  if (!m_handler) {
533  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
534  }
535 
536  franka::RobotState robot_state = getRobotInternalState();
537 
538  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
539  fJe.resize(6, 7); // row-major
540  for (size_t i = 0; i < 6; i ++) {
541  for (size_t j = 0; j < 7; j ++) {
542  fJe[i][j] = jacobian[j*6 + i];
543  }
544  }
545  // TODO check from vpRobot fJe and fJeAvailable
546 }
547 
555 {
556  if (!m_handler) {
557  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
558  }
559  if (q.size() != 7) {
560  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector", q.size()));
561  }
562 
563  franka::RobotState robot_state = getRobotInternalState();
564 
565  std::array< double, 7 > q_array;
566  for (size_t i = 0; i < 7; i++)
567  q_array[i] = q[i];
568 
569  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
570  fJe.resize(6, 7); // row-major
571  for (size_t i = 0; i < 6; i ++) {
572  for (size_t j = 0; j < 7; j ++) {
573  fJe[i][j] = jacobian[j*6 + i];
574  }
575  }
576  // TODO check from vpRobot fJe and fJeAvailable
577 }
578 
588 void vpRobotFranka::setLogFolder(const std::string &folder)
589 {
590  if (!folder.empty()) {
591  if (vpIoTools::checkDirectory(folder) == false) {
592  try {
593  vpIoTools::makeDirectory(folder);
594  m_log_folder = folder;
595  }
596  catch(const vpException &e) {
597  std::string error;
598  error = "Unable to create Franka log folder: " + folder;
599  throw(vpException(vpException::fatalError, error));
600  }
601  }
602  else {
603  m_log_folder = folder;
604  }
605  }
606 }
607 
614 {
615  if (!m_handler) {
616  throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
617  }
619  std::cout << "Robot was not in position-based control. "
620  "Modification of the robot state" << std::endl;
622  }
623 
624  if (frame == vpRobot::JOINT_STATE) {
625  double speed_factor = m_positionningVelocity / 100.;
626 
627  std::array<double, 7> q_goal;
628  for (size_t i = 0; i < 7; i++) {
629  q_goal[i] = position[i];
630  }
631 
632  vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
633 
634  int nbAttempts = 10;
635  for (int attempt = 1; attempt <= nbAttempts; attempt++) {
636  try {
637  m_handler->control(joint_pos_traj_generator);
638  break;
639  } catch (const franka::ControlException &e) {
640  std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
641  m_handler->automaticErrorRecovery();
642  if (attempt == nbAttempts)
643  throw;
644  }
645  }
646  }
647  else {
649  "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
650  }
651 }
652 
661 void vpRobotFranka::setPositioningVelocity(const double velocity)
662 {
663  m_positionningVelocity = velocity;
664 }
665 
673 {
674  switch (newState) {
675  case vpRobot::STATE_STOP: {
676  // Start primitive STOP only if the current state is Velocity
678  // Stop the robot
679  m_controlThreadStopAsked = true;
680  if(m_controlThread.joinable()) {
681  m_controlThread.join();
682  m_controlThreadStopAsked = false;
683  m_controlThreadIsRunning = false;
684  }
685  }
686  break;
687  }
690  std::cout << "Change the control mode from velocity to position control.\n";
691  // Stop the robot
692  m_controlThreadStopAsked = true;
693  if(m_controlThread.joinable()) {
694  m_controlThread.join();
695  m_controlThreadStopAsked = false;
696  m_controlThreadIsRunning = false;
697  }
698  }
699  break;
700  }
703  std::cout << "Change the control mode from stop to velocity control.\n";
704  }
705  break;
706  }
707  default:
708  break;
709  }
710 
711  return vpRobot::setRobotState(newState);
712 }
713 
768 {
771  "Cannot send a velocity to the robot. "
772  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
773  }
774 
775  switch (frame) {
776  // Saturation in joint space
777  case JOINT_STATE: {
778  if (vel.size() != 7) {
780  "Joint velocity vector (%d) is not of size 7", vel.size());
781  }
782 
783  vpColVector vel_max(7, getMaxRotationVelocity());
784 
785  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
786 
787  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
788  m_dq_des[i] = vel_sat[i];
789  }
790 
791  break;
792  }
793 
794  // Saturation in cartesian space
795  case vpRobot::TOOL_FRAME:
798  if (vel.size() != 6) {
800  "Cartesian velocity vector (%d) is not of size 6", vel.size());
801  }
802  vpColVector vel_max(6);
803 
804  for (unsigned int i = 0; i < 3; i++)
805  vel_max[i] = getMaxTranslationVelocity();
806  for (unsigned int i = 3; i < 6; i++)
807  vel_max[i] = getMaxRotationVelocity();
808 
809  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
810 
811  break;
812  }
813 
814  case vpRobot::MIXT_FRAME: {
816  "Velocity controller not supported");
817  }
818  }
819 
820  if(! m_controlThreadIsRunning) {
821  m_controlThreadIsRunning = true;
822  m_controlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
823  std::ref(m_handler), std::ref(m_controlThreadStopAsked), m_log_folder,
824  frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
825  std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
826  std::ref(m_robot_state), std::ref(m_mutex));
827  }
828 }
829 
830 franka::RobotState vpRobotFranka::getRobotInternalState()
831 {
832  if (!m_handler) {
833  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
834  }
835  franka::RobotState robot_state;
836 
837  if (! m_controlThreadIsRunning) {
838  robot_state = m_handler->readOnce();
839 
840  std::lock_guard<std::mutex> lock(m_mutex);
841  m_robot_state = robot_state;
842  }
843  else { // robot_state is updated in the velocity control thread
844  std::lock_guard<std::mutex> lock(m_mutex);
845  robot_state = m_robot_state;
846  }
847 
848  return robot_state;
849 }
850 
857 {
858  vpColVector q_min(m_q_min.size());
859  for (size_t i = 0; i < m_q_min.size(); i ++)
860  q_min[i] = m_q_min[i];
861 
862  return q_min;
863 }
870 {
871  vpColVector q_max(m_q_max.size());
872  for (size_t i = 0; i < m_q_max.size(); i ++)
873  q_max[i] = m_q_max[i];
874 
875  return q_max;
876 }
877 
889 {
890  return m_eMc;
891 }
892 
906 {
907  m_eMc = eMc;
908 }
909 
919 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
920 {
921  vpColVector q;
922 
923  this->readPosFile(filename, q);
925  this->setPositioningVelocity(velocity_percentage);
927 }
928 
973 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
974 {
975  std::ifstream fd(filename.c_str(), std::ios::in);
976 
977  if (!fd.is_open()) {
978  return false;
979  }
980 
981  std::string line;
982  std::string key("R:");
983  std::string id("#PANDA - Joint position file");
984  bool pos_found = false;
985  int lineNum = 0;
986  size_t njoints = 7;
987 
988  q.resize(njoints);
989 
990  while (std::getline(fd, line)) {
991  lineNum++;
992  if (lineNum == 1) {
993  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
994  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
995  return false;
996  }
997  }
998  if ((line.compare(0, 1, "#") == 0)) { // skip comment
999  continue;
1000  }
1001  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1002  // check if there are at least njoint values in the line
1003  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1004  if (chain.size() < njoints + 1) // try to split with tab separator
1005  chain = vpIoTools::splitChain(line, std::string("\t"));
1006  if (chain.size() < njoints + 1)
1007  continue;
1008 
1009  std::istringstream ss(line);
1010  std::string key_;
1011  ss >> key_;
1012  for (unsigned int i = 0; i < njoints; i++)
1013  ss >> q[i];
1014  pos_found = true;
1015  break;
1016  }
1017  }
1018 
1019  // converts rotations from degrees into radians
1020  for (unsigned int i = 0; i < njoints; i++) {
1021  q[i] = vpMath::rad(q[i]);
1022  }
1023 
1024  fd.close();
1025 
1026  if (!pos_found) {
1027  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1028  return false;
1029  }
1030 
1031  return true;
1032 }
1033 
1056 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1057 {
1058 
1059  FILE *fd;
1060  fd = fopen(filename.c_str(), "w");
1061  if (fd == NULL)
1062  return false;
1063 
1064  fprintf(fd,
1065  "#PANDA - Joint position file\n"
1066  "#\n"
1067  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1068  "# with joint positions q1 to q7 expressed in degrees\n"
1069  "#\n");
1070 
1071  // Save positions in mm and deg
1072  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1073  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1074 
1075  fclose(fd);
1076  return (true);
1077 }
1078 
1087 {
1089  vpColVector q(7, 0);
1091  }
1093 }
1094 
1102 {
1103  if (m_franka_address.empty()) {
1104  throw (vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1105  }
1106  if (m_gripper == NULL)
1107  m_gripper = new franka::Gripper(m_franka_address);
1108 
1109  m_gripper->homing();
1110 }
1111 
1122 {
1123  if (m_franka_address.empty()) {
1124  throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1125  }
1126  if (m_gripper == NULL)
1127  m_gripper = new franka::Gripper(m_franka_address);
1128 
1129  // Check for the maximum grasping width.
1130  franka::GripperState gripper_state = m_gripper->readOnce();
1131 
1132  if (gripper_state.max_width < width) {
1133  std::cout << "Finger width request is too large for the current fingers on the gripper."
1134  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1135  return EXIT_FAILURE;
1136  }
1137 
1138  m_gripper->move(width, 0.1);
1139  return EXIT_SUCCESS;
1140 }
1141 
1151 {
1152  return gripperMove(0);
1153 }
1154 
1164 {
1165  if (m_franka_address.empty()) {
1166  throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1167  }
1168  if (m_gripper == NULL)
1169  m_gripper = new franka::Gripper(m_franka_address);
1170 
1171  // Check for the maximum grasping width.
1172  franka::GripperState gripper_state = m_gripper->readOnce();
1173 
1174  m_gripper->move(gripper_state.max_width, 0.1);
1175  return EXIT_SUCCESS;
1176 }
1177 
1185 {
1186  if (m_franka_address.empty()) {
1187  throw (vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1188  }
1189  if (m_gripper == NULL)
1190  m_gripper = new franka::Gripper(m_franka_address);
1191 
1192  m_gripper->stop();
1193 }
1194 
1209 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1210 {
1211  if (m_gripper == NULL)
1212  m_gripper = new franka::Gripper(m_franka_address);
1213 
1214  // Check for the maximum grasping width.
1215  franka::GripperState gripper_state = m_gripper->readOnce();
1216  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1217  if (gripper_state.max_width < grasping_width) {
1218  std::cout << "Object is too large for the current fingers on the gripper."
1219  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1220  return EXIT_FAILURE;
1221  }
1222 
1223  // Grasp the object.
1224  if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1225  std::cout << "Failed to grasp object." << std::endl;
1226  return EXIT_FAILURE;
1227  }
1228 
1229  return EXIT_SUCCESS;
1230 }
1231 
1232 #elif !defined(VISP_BUILD_SHARED_LIBS)
1233 // Work arround to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1234 // no symbols
1235 void dummy_vpRobotFranka(){};
1236 #endif
1237 
int gripperMove(double width)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpColVector getJointMax() const
Error that can be emited by the vpRobot class and its derivates.
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:467
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void getCoriolis(vpColVector &coriolis)
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool savePosFile(const std::string &filename, const vpColVector &q)
virtual ~vpRobotFranka()
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
vpHomogeneousMatrix get_fMe(const vpColVector &q)
void getGravity(vpColVector &gravity)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
vpHomogeneousMatrix get_eMc() const
Initialize the position controller.
Definition: vpRobot.h:68
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
int gripperGrasp(double grasping_width, double force=60.)
void getMass(vpMatrix &mass)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void set_eMc(const vpHomogeneousMatrix &eMc)
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:597
Initialize the velocity controller.
Definition: vpRobot.h:67
vpRobotStateType
Definition: vpRobot.h:64
bool readPosFile(const std::string &filename, vpColVector &q)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1771
void get_eJe(vpMatrix &eJe)
static double rad(double deg)
Definition: vpMath.h:102
void get_fJe(vpMatrix &fJe)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static double deg(double rad)
Definition: vpMath.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
void move(const std::string &filename, double velocity_percentage=10.)
void setPositioningVelocity(const double velocity)
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Function not implemented.
Definition: vpException.h:90
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244