Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
vpRobotFranka.cpp
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 Franka robot.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_FRANKA
39 
40 #include <visp3/core/vpIoTools.h>
41 #include <visp3/robot/vpRobotException.h>
42 #include <visp3/robot/vpRobotFranka.h>
43 
44 #include "vpForceTorqueGenerator_impl.h"
45 #include "vpJointPosTrajGenerator_impl.h"
46 #include "vpJointVelTrajGenerator_impl.h"
47 
54  : vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
55  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
56  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
57  m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
58  m_franka_address()
59 {
60  init();
61 }
62 
69 vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
70  : vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
71  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
72  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
73  m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
74  m_franka_address()
75 {
76  init();
77  connect(franka_address, realtime_config);
78 }
79 
83 void vpRobotFranka::init()
84 {
85  nDof = 7;
86 
87  m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
88  m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
89  m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
90  m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
91 }
92 
99 {
101 
102  if (m_handler)
103  delete m_handler;
104 
105  if (m_gripper) {
106  std::cout << "Grasped object, will release it now." << std::endl;
107  m_gripper->stop();
108  delete m_gripper;
109  }
110 
111  if (m_model) {
112  delete m_model;
113  }
114 }
115 
122 void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
123 {
124  init();
125  if (franka_address.empty()) {
126  throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
127  }
128  if (m_handler)
129  delete m_handler;
130 
131  m_franka_address = franka_address;
132  m_handler = new franka::Robot(franka_address, realtime_config);
133 
134  std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
135  std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
136  std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
137  std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
138  std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
139  std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
140  std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
141  std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
142  m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
143  lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
144  lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
145  lower_force_thresholds_nominal, upper_force_thresholds_nominal);
146 
147  m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
148  m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
149 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
150  // m_handler->setFilters(100, 100, 100, 100, 100);
151  m_handler->setFilters(10, 10, 10, 10, 10);
152 #else
153  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
154 #endif
155  if (m_model) {
156  delete m_model;
157  }
158  m_model = new franka::Model(m_handler->loadModel());
159 }
160 
176 {
177  if (!m_handler) {
178  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
179  }
180 
181  franka::RobotState robot_state = getRobotInternalState();
182  vpColVector q(7);
183  for (int i = 0; i < 7; i++) {
184  q[i] = robot_state.q[i];
185  }
186 
187  switch (frame) {
188  case JOINT_STATE: {
189  position = q;
190  break;
191  }
192  case END_EFFECTOR_FRAME: {
193  position.resize(6);
194  vpHomogeneousMatrix fMe = get_fMe(q);
195  vpPoseVector fPe(fMe);
196  for (size_t i = 0; i < 6; i++) {
197  position[i] = fPe[i];
198  }
199  break;
200  }
201  case TOOL_FRAME: { // same a CAMERA_FRAME
202  position.resize(6);
203  vpHomogeneousMatrix fMc = get_fMc(q);
204  vpPoseVector fPc(fMc);
205  for (size_t i = 0; i < 6; i++) {
206  position[i] = fPc[i];
207  }
208  break;
209  }
210  default: {
211  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
212  }
213  }
214 }
215 
227 {
228  if (!m_handler) {
229  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
230  }
231 
232  franka::RobotState robot_state = getRobotInternalState();
233 
234  switch (frame) {
235  case JOINT_STATE: {
236  force.resize(7);
237  for (int i = 0; i < 7; i++)
238  force[i] = robot_state.tau_J[i];
239 
240  break;
241  }
242  case END_EFFECTOR_FRAME: {
243  force.resize(6);
244  for (int i = 0; i < 6; i++)
245  force[i] = robot_state.K_F_ext_hat_K[i];
246  break;
247  }
248  case TOOL_FRAME: {
249  // end-effector frame
250  vpColVector eFe(6);
251  for (int i = 0; i < 6; i++)
252  eFe[i] = robot_state.K_F_ext_hat_K[i];
253 
254  // Transform in tool frame
256  vpForceTwistMatrix cWe(cMe);
257  force = cWe * eFe;
258  break;
259  }
260  default: {
261  throw(vpException(vpException::fatalError, "Cannot get Franka force/torque: frame not supported"));
262  }
263  }
264 }
265 
281 {
282  if (!m_handler) {
283  throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
284  }
285 
286  franka::RobotState robot_state = getRobotInternalState();
287 
288  switch (frame) {
289 
290  case JOINT_STATE: {
291  d_position.resize(7);
292  for (int i = 0; i < 7; i++) {
293  d_position[i] = robot_state.dq[i];
294  }
295  break;
296  }
297 
298  default: {
299  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
300  }
301  }
302 }
303 
310 {
311  if (!m_handler) {
312  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
313  }
314 
315  std::array<double, 7> coriolis_;
316 
317  franka::RobotState robot_state = getRobotInternalState();
318 
319  coriolis_ = m_model->coriolis(robot_state);
320 
321  coriolis.resize(7);
322  for (int i = 0; i < 7; i++) {
323  coriolis[i] = coriolis_[i];
324  }
325 }
326 
332 {
333  if (!m_handler) {
334  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
335  }
336 
337  std::array<double, 7> gravity_;
338 
339  franka::RobotState robot_state = getRobotInternalState();
340 
341  gravity_ = m_model->gravity(robot_state);
342 
343  gravity.resize(7);
344  for (int i = 0; i < 7; i++) {
345  gravity[i] = gravity_[i];
346  }
347 }
348 
354 {
355  if (!m_handler) {
356  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
357  }
358 
359  std::array<double, 49> mass_;
360 
361  franka::RobotState robot_state = getRobotInternalState();
362 
363  mass_ = m_model->mass(robot_state); // column-major
364 
365  mass.resize(7, 7); // row-major
366  for (size_t i = 0; i < 7; i++) {
367  for (size_t j = 0; j < 7; j++) {
368  mass[i][j] = mass_[j * 7 + i];
369  }
370  }
371 }
372 
381 {
382  if (!m_handler) {
383  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
384  }
385  if (q.size() != 7) {
386  throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
387  }
388 
389  std::array<double, 7> q_array;
390  for (size_t i = 0; i < 7; i++)
391  q_array[i] = q[i];
392 
393  franka::RobotState robot_state = getRobotInternalState();
394 
395  std::array<double, 16> pose_array =
396  m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
398  for (unsigned int i = 0; i < 4; i++) {
399  for (unsigned int j = 0; j < 4; j++) {
400  fMe[i][j] = pose_array[j * 4 + i];
401  }
402  }
403 
404  return fMe;
405 }
406 
422 {
423  vpHomogeneousMatrix fMe = get_fMe(q);
424  return (fMe * m_eMc);
425 }
426 
437 {
438  if (!m_handler) {
439  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
440  }
441  if (frame == JOINT_STATE) {
442  throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
443  }
444 
445  franka::RobotState robot_state = getRobotInternalState();
446 
447  std::array<double, 16> pose_array = robot_state.O_T_EE;
449  for (unsigned int i = 0; i < 4; i++) {
450  for (unsigned int j = 0; j < 4; j++) {
451  fMe[i][j] = pose_array[j * 4 + i];
452  }
453  }
454 
455  switch (frame) {
456  case END_EFFECTOR_FRAME: {
457  pose.buildFrom(fMe);
458  break;
459  }
460  case TOOL_FRAME: {
461  pose.buildFrom(fMe * m_eMc);
462  break;
463  }
464  default: {
465  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
466  }
467  }
468 }
469 
476 {
477  if (!m_handler) {
478  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
479  }
480 
481  franka::RobotState robot_state = getRobotInternalState();
482 
483  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
484  eJe_.resize(6, 7); // row-major
485  for (size_t i = 0; i < 6; i++) {
486  for (size_t j = 0; j < 7; j++) {
487  eJe_[i][j] = jacobian[j * 6 + i];
488  }
489  }
490  // TODO check from vpRobot fJe and fJeAvailable
491 }
492 
499 {
500  if (!m_handler) {
501  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
502  }
503 
504  franka::RobotState robot_state = getRobotInternalState();
505 
506  std::array<double, 7> q_array;
507  for (size_t i = 0; i < 7; i++)
508  q_array[i] = q[i];
509 
510  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
511  robot_state.EE_T_K); // column-major
512  eJe_.resize(6, 7); // row-major
513  for (size_t i = 0; i < 6; i++) {
514  for (size_t j = 0; j < 7; j++) {
515  eJe_[i][j] = jacobian[j * 6 + i];
516  }
517  }
518  // TODO check from vpRobot fJe and fJeAvailable
519 }
520 
527 {
528  if (!m_handler) {
529  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
530  }
531 
532  franka::RobotState robot_state = getRobotInternalState();
533 
534  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
535  fJe_.resize(6, 7); // row-major
536  for (size_t i = 0; i < 6; i++) {
537  for (size_t j = 0; j < 7; j++) {
538  fJe_[i][j] = jacobian[j * 6 + i];
539  }
540  }
541  // TODO check from vpRobot fJe and fJeAvailable
542 }
543 
550 {
551  if (!m_handler) {
552  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
553  }
554  if (q.size() != 7) {
555  throw(vpException(
557  "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
558  q.size()));
559  }
560 
561  franka::RobotState robot_state = getRobotInternalState();
562 
563  std::array<double, 7> q_array;
564  for (size_t i = 0; i < 7; i++)
565  q_array[i] = q[i];
566 
567  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
568  robot_state.EE_T_K); // column-major
569  fJe_.resize(6, 7); // row-major
570  for (size_t i = 0; i < 6; i++) {
571  for (size_t j = 0; j < 7; j++) {
572  fJe_[i][j] = jacobian[j * 6 + i];
573  }
574  }
575  // TODO check from vpRobot fJe and fJeAvailable
576 }
577 
587 void vpRobotFranka::setLogFolder(const std::string &folder)
588 {
589  if (!folder.empty()) {
590  if (vpIoTools::checkDirectory(folder) == false) {
591  try {
592  vpIoTools::makeDirectory(folder);
593  m_log_folder = folder;
594  } catch (const vpException &e) {
595  std::string error;
596  error = "Unable to create Franka log folder: " + folder;
597  throw(vpException(vpException::fatalError, error));
598  }
599  } else {
600  m_log_folder = folder;
601  }
602  }
603 }
604 
611 {
612  if (!m_handler) {
613  throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
614  }
616  std::cout << "Robot was not in position-based control. "
617  "Modification of the robot state"
618  << std::endl;
620  }
621 
622  if (frame == vpRobot::JOINT_STATE) {
623  double speed_factor = m_positioningVelocity / 100.;
624 
625  std::array<double, 7> q_goal;
626  for (size_t i = 0; i < 7; i++) {
627  q_goal[i] = position[i];
628  }
629 
630  vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
631 
632  int nbAttempts = 10;
633  for (int attempt = 1; attempt <= nbAttempts; attempt++) {
634  try {
635  m_handler->control(joint_pos_traj_generator);
636  break;
637  } catch (const franka::ControlException &e) {
638  std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
639  m_handler->automaticErrorRecovery();
640  if (attempt == nbAttempts)
641  throw;
642  }
643  }
644  } else {
646  "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
647  }
648 }
649 
658 void vpRobotFranka::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
659 
667 {
668  switch (newState) {
669  case vpRobot::STATE_STOP: {
670  // Start primitive STOP only if the current state is velocity or force/torque
672  // Stop the robot
673  m_velControlThreadStopAsked = true;
674  if (m_velControlThread.joinable()) {
675  m_velControlThread.join();
676  m_velControlThreadStopAsked = false;
677  m_velControlThreadIsRunning = false;
678  }
680  // Stop the robot
681  m_ftControlThreadStopAsked = true;
682  if (m_ftControlThread.joinable()) {
683  m_ftControlThread.join();
684  m_ftControlThreadStopAsked = false;
685  m_ftControlThreadIsRunning = false;
686  }
687  }
688  break;
689  }
692  std::cout << "Change the control mode from velocity to position control.\n";
693  // Stop the robot
694  m_velControlThreadStopAsked = true;
695  if (m_velControlThread.joinable()) {
696  m_velControlThread.join();
697  m_velControlThreadStopAsked = false;
698  m_velControlThreadIsRunning = false;
699  }
701  std::cout << "Change the control mode from force/torque to position control.\n";
702  // Stop the robot
703  m_ftControlThreadStopAsked = true;
704  if (m_ftControlThread.joinable()) {
705  m_ftControlThread.join();
706  m_ftControlThreadStopAsked = false;
707  m_ftControlThreadIsRunning = false;
708  }
709  }
710  break;
711  }
714  std::cout << "Change the control mode from stop to velocity control.\n";
716  std::cout << "Change the control mode from position to velocity control.\n";
718  std::cout << "Change the control mode from force/torque to velocity control.\n";
719  // Stop the robot
720  m_ftControlThreadStopAsked = true;
721  if (m_ftControlThread.joinable()) {
722  m_ftControlThread.join();
723  m_ftControlThreadStopAsked = false;
724  m_ftControlThreadIsRunning = false;
725  }
726  }
727  break;
728  }
731  std::cout << "Change the control mode from stop to force/torque control.\n";
733  std::cout << "Change the control mode from position to force/torque control.\n";
735  std::cout << "Change the control mode from velocity to force/torque control.\n";
736  // Stop the robot
737  m_velControlThreadStopAsked = true;
738  if (m_velControlThread.joinable()) {
739  m_velControlThread.join();
740  m_velControlThreadStopAsked = false;
741  m_velControlThreadIsRunning = false;
742  }
743  }
744  break;
745  }
746 
747  default:
748  break;
749  }
750 
751  return vpRobot::setRobotState(newState);
752 }
753 
808 {
811  "Cannot send a velocity to the robot. "
812  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
813  }
814 
815  switch (frame) {
816  // Saturation in joint space
817  case JOINT_STATE: {
818  if (vel.size() != 7) {
819  throw vpRobotException(vpRobotException::wrongStateError, "Joint velocity vector (%d) is not of size 7",
820  vel.size());
821  }
822 
823  vpColVector vel_max(7, getMaxRotationVelocity());
824 
825  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
826 
827  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
828  m_dq_des[i] = vel_sat[i];
829  }
830 
831  break;
832  }
833 
834  // Saturation in cartesian space
835  case vpRobot::TOOL_FRAME:
838  if (vel.size() != 6) {
839  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian velocity vector (%d) is not of size 6",
840  vel.size());
841  }
842  vpColVector vel_max(6);
843 
844  for (unsigned int i = 0; i < 3; i++)
845  vel_max[i] = getMaxTranslationVelocity();
846  for (unsigned int i = 3; i < 6; i++)
847  vel_max[i] = getMaxRotationVelocity();
848 
849  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
850 
851  break;
852  }
853 
854  case vpRobot::MIXT_FRAME: {
855  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
856  }
857  }
858 
859  if (!m_velControlThreadIsRunning) {
860  m_velControlThreadIsRunning = true;
861  m_velControlThread =
862  std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
863  std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
864  std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
865  std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
866  }
867 }
868 
869 /*
870  Apply a force/torque to the robot.
871 
872  \param[in] frame : Control frame in which the force/torque is applied.
873 
874  \param[in] ft : Force/torque vector. The size of this vector
875  is always 6 for a cartesian force/torque skew, and 7 for joint torques.
876 
877  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
878  To diable the filter set filter_gain to 1.
879  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
880  */
882  const double &filter_gain, const bool &activate_pi_controller)
883 {
884  switch (frame) {
885  // Saturation in joint space
886  case JOINT_STATE: {
887  if (ft.size() != 7) {
888  throw vpRobotException(vpRobotException::wrongStateError, "Joint torques vector (%d) is not of size 7",
889  ft.size());
890  }
891 
892  for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
893  m_tau_J_des[i] = ft[i];
894  }
895  // TODO: Introduce force/torque saturation
896 
897  break;
898  }
899 
900  // Saturation in cartesian space
901  case vpRobot::TOOL_FRAME:
904  if (ft.size() != 6) {
905  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian force/torque vector (%d) is not of size 6",
906  ft.size());
907  }
908 
909  m_ft_cart_des = ft;
910  // TODO: Introduce force/torque saturation
911 
912  break;
913  }
914 
915  case vpRobot::MIXT_FRAME: {
916  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
917  }
918  }
919 
920  if (!m_ftControlThreadIsRunning) {
921  getRobotInternalState(); // Update m_robot_state internally
922  m_ftControlThreadIsRunning = true;
923  m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
924  std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
925  std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
926  std::ref(m_mutex), filter_gain, activate_pi_controller);
927  }
928 }
929 
931 {
932  if (!m_handler) {
933  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
934  }
935  franka::RobotState robot_state;
936 
937  if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
938  robot_state = m_handler->readOnce();
939 
940  std::lock_guard<std::mutex> lock(m_mutex);
941  m_robot_state = robot_state;
942  } else { // robot_state is updated in the velocity control thread
943  std::lock_guard<std::mutex> lock(m_mutex);
944  robot_state = m_robot_state;
945  }
946 
947  return robot_state;
948 }
949 
956 {
957  vpColVector q_min(m_q_min.size());
958  for (size_t i = 0; i < m_q_min.size(); i++)
959  q_min[i] = m_q_min[i];
960 
961  return q_min;
962 }
969 {
970  vpColVector q_max(m_q_max.size());
971  for (size_t i = 0; i < m_q_max.size(); i++)
972  q_max[i] = m_q_max[i];
973 
974  return q_max;
975 }
976 
988 
1001 void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc) { m_eMc = eMc; }
1002 
1012 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1013 {
1014  vpColVector q;
1015 
1016  this->readPosFile(filename, q);
1018  this->setPositioningVelocity(velocity_percentage);
1020 }
1021 
1066 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1067 {
1068  std::ifstream fd(filename.c_str(), std::ios::in);
1069 
1070  if (!fd.is_open()) {
1071  return false;
1072  }
1073 
1074  std::string line;
1075  std::string key("R:");
1076  std::string id("#PANDA - Joint position file");
1077  bool pos_found = false;
1078  int lineNum = 0;
1079  size_t njoints = 7;
1080 
1081  q.resize(njoints);
1082 
1083  while (std::getline(fd, line)) {
1084  lineNum++;
1085  if (lineNum == 1) {
1086  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1087  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1088  return false;
1089  }
1090  }
1091  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1092  continue;
1093  }
1094  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1095  // check if there are at least njoint values in the line
1096  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1097  if (chain.size() < njoints + 1) // try to split with tab separator
1098  chain = vpIoTools::splitChain(line, std::string("\t"));
1099  if (chain.size() < njoints + 1)
1100  continue;
1101 
1102  std::istringstream ss(line);
1103  std::string key_;
1104  ss >> key_;
1105  for (unsigned int i = 0; i < njoints; i++)
1106  ss >> q[i];
1107  pos_found = true;
1108  break;
1109  }
1110  }
1111 
1112  // converts rotations from degrees into radians
1113  for (unsigned int i = 0; i < njoints; i++) {
1114  q[i] = vpMath::rad(q[i]);
1115  }
1116 
1117  fd.close();
1118 
1119  if (!pos_found) {
1120  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1121  return false;
1122  }
1123 
1124  return true;
1125 }
1126 
1149 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1150 {
1151 
1152  FILE *fd;
1153  fd = fopen(filename.c_str(), "w");
1154  if (fd == nullptr)
1155  return false;
1156 
1157  fprintf(fd, "#PANDA - Joint position file\n"
1158  "#\n"
1159  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1160  "# with joint positions q1 to q7 expressed in degrees\n"
1161  "#\n");
1162 
1163  // Save positions in mm and deg
1164  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1165  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1166 
1167  fclose(fd);
1168  return (true);
1169 }
1170 
1179 {
1181  vpColVector q(7, 0);
1183  }
1185 }
1186 
1194 {
1195  if (m_franka_address.empty()) {
1196  throw(vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1197  }
1198  if (m_gripper == nullptr)
1199  m_gripper = new franka::Gripper(m_franka_address);
1200 
1201  m_gripper->homing();
1202 }
1203 
1214 {
1215  if (m_franka_address.empty()) {
1216  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1217  }
1218  if (m_gripper == nullptr)
1219  m_gripper = new franka::Gripper(m_franka_address);
1220 
1221  // Check for the maximum grasping width.
1222  franka::GripperState gripper_state = m_gripper->readOnce();
1223 
1224  if (gripper_state.max_width < width) {
1225  std::cout << "Finger width request is too large for the current fingers on the gripper."
1226  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1227  return EXIT_FAILURE;
1228  }
1229 
1230  m_gripper->move(width, 0.1);
1231  return EXIT_SUCCESS;
1232 }
1233 
1243 
1253 {
1254  if (m_franka_address.empty()) {
1255  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1256  }
1257  if (m_gripper == nullptr)
1258  m_gripper = new franka::Gripper(m_franka_address);
1259 
1260  // Check for the maximum grasping width.
1261  franka::GripperState gripper_state = m_gripper->readOnce();
1262 
1263  m_gripper->move(gripper_state.max_width, 0.1);
1264  return EXIT_SUCCESS;
1265 }
1266 
1274 {
1275  if (m_franka_address.empty()) {
1276  throw(vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1277  }
1278  if (m_gripper == nullptr)
1279  m_gripper = new franka::Gripper(m_franka_address);
1280 
1281  m_gripper->stop();
1282 }
1283 
1298 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1299 {
1300  if (m_gripper == nullptr)
1301  m_gripper = new franka::Gripper(m_franka_address);
1302 
1303  // Check for the maximum grasping width.
1304  franka::GripperState gripper_state = m_gripper->readOnce();
1305  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1306  if (gripper_state.max_width < grasping_width) {
1307  std::cout << "Object is too large for the current fingers on the gripper."
1308  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1309  return EXIT_FAILURE;
1310  }
1311 
1312  // Grasp the object.
1313  if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1314  std::cout << "Failed to grasp object." << std::endl;
1315  return EXIT_FAILURE;
1316  }
1317 
1318  return EXIT_SUCCESS;
1319 }
1320 
1321 #elif !defined(VISP_BUILD_SHARED_LIBS)
1322 // Work around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1323 // no symbols
1324 void dummy_vpRobotFranka(){};
1325 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:282
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:269
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1049
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ fatalError
Fatal error.
Definition: vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2000
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:449
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:603
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
franka::RobotState getRobotInternalState()
void move(const std::string &filename, double velocity_percentage=10.)
bool savePosFile(const std::string &filename, const vpColVector &q)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpHomogeneousMatrix get_eMc() const
int gripperGrasp(double grasping_width, double force=60.)
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
void getGravity(vpColVector &gravity)
int gripperMove(double width)
void getMass(vpMatrix &mass)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) override
void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft, const double &filter_gain=0.1, const bool &activate_pi_controller=false)
vpColVector getJointMax() const
void getCoriolis(vpColVector &coriolis)
void setPositioningVelocity(double velocity)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) override
void get_fJe(vpMatrix &fJe) override
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) override
void get_eJe(vpMatrix &eJe) override
virtual ~vpRobotFranka()
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
vpHomogeneousMatrix get_fMe(const vpColVector &q)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
@ STATE_FORCE_TORQUE_CONTROL
Initialize the force/torque controller.
Definition: vpRobot.h:68
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248