Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
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 #if defined(VISP_HAVE_FRANKA) && defined(VISP_HAVE_THREADS)
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  }
595  catch (const vpException &e) {
596  std::string error;
597  error = "Unable to create Franka log folder: " + folder;
598  throw(vpException(vpException::fatalError, error));
599  }
600  }
601  else {
602  m_log_folder = folder;
603  }
604  }
605 }
606 
613 {
614  if (!m_handler) {
615  throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
616  }
618  std::cout << "Robot was not in position-based control. "
619  "Modification of the robot state"
620  << std::endl;
622  }
623 
624  if (frame == vpRobot::JOINT_STATE) {
625  double speed_factor = m_positioningVelocity / 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  }
640  catch (const franka::ControlException &e) {
641  std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
642  m_handler->automaticErrorRecovery();
643  if (attempt == nbAttempts)
644  throw;
645  }
646  }
647  }
648  else {
650  "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
651  }
652 }
653 
662 void vpRobotFranka::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
663 
671 {
672  switch (newState) {
673  case vpRobot::STATE_STOP: {
674  // Start primitive STOP only if the current state is velocity or force/torque
676  // Stop the robot
677  m_velControlThreadStopAsked = true;
678  if (m_velControlThread.joinable()) {
679  m_velControlThread.join();
680  m_velControlThreadStopAsked = false;
681  m_velControlThreadIsRunning = false;
682  }
683  }
685  // Stop the robot
686  m_ftControlThreadStopAsked = true;
687  if (m_ftControlThread.joinable()) {
688  m_ftControlThread.join();
689  m_ftControlThreadStopAsked = false;
690  m_ftControlThreadIsRunning = false;
691  }
692  }
693  break;
694  }
697  std::cout << "Change the control mode from velocity to position control.\n";
698  // Stop the robot
699  m_velControlThreadStopAsked = true;
700  if (m_velControlThread.joinable()) {
701  m_velControlThread.join();
702  m_velControlThreadStopAsked = false;
703  m_velControlThreadIsRunning = false;
704  }
705  }
707  std::cout << "Change the control mode from force/torque to position control.\n";
708  // Stop the robot
709  m_ftControlThreadStopAsked = true;
710  if (m_ftControlThread.joinable()) {
711  m_ftControlThread.join();
712  m_ftControlThreadStopAsked = false;
713  m_ftControlThreadIsRunning = false;
714  }
715  }
716  break;
717  }
720  std::cout << "Change the control mode from stop to velocity control.\n";
721  }
723  std::cout << "Change the control mode from position to velocity control.\n";
724  }
726  std::cout << "Change the control mode from force/torque to velocity control.\n";
727  // Stop the robot
728  m_ftControlThreadStopAsked = true;
729  if (m_ftControlThread.joinable()) {
730  m_ftControlThread.join();
731  m_ftControlThreadStopAsked = false;
732  m_ftControlThreadIsRunning = false;
733  }
734  }
735  break;
736  }
739  std::cout << "Change the control mode from stop to force/torque control.\n";
740  }
742  std::cout << "Change the control mode from position to force/torque control.\n";
743  }
745  std::cout << "Change the control mode from velocity to force/torque control.\n";
746  // Stop the robot
747  m_velControlThreadStopAsked = true;
748  if (m_velControlThread.joinable()) {
749  m_velControlThread.join();
750  m_velControlThreadStopAsked = false;
751  m_velControlThreadIsRunning = false;
752  }
753  }
754  break;
755  }
756 
757  default:
758  break;
759  }
760 
761  return vpRobot::setRobotState(newState);
762 }
763 
818 {
821  "Cannot send a velocity to the robot. "
822  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
823  }
824 
825  switch (frame) {
826  // Saturation in joint space
827  case JOINT_STATE: {
828  if (vel.size() != 7) {
829  throw vpRobotException(vpRobotException::wrongStateError, "Joint velocity vector (%d) is not of size 7",
830  vel.size());
831  }
832 
833  vpColVector vel_max(7, getMaxRotationVelocity());
834 
835  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
836 
837  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
838  m_dq_des[i] = vel_sat[i];
839  }
840 
841  break;
842  }
843 
844  // Saturation in cartesian space
845  case vpRobot::TOOL_FRAME:
848  if (vel.size() != 6) {
849  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian velocity vector (%d) is not of size 6",
850  vel.size());
851  }
852  vpColVector vel_max(6);
853 
854  for (unsigned int i = 0; i < 3; i++)
855  vel_max[i] = getMaxTranslationVelocity();
856  for (unsigned int i = 3; i < 6; i++)
857  vel_max[i] = getMaxRotationVelocity();
858 
859  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
860 
861  break;
862  }
863 
864  case vpRobot::MIXT_FRAME: {
865  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
866  }
867  }
868 
869  if (!m_velControlThreadIsRunning) {
870  m_velControlThreadIsRunning = true;
871  m_velControlThread =
872  std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
873  std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
874  std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
875  std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
876  }
877 }
878 
879 /*
880  Apply a force/torque to the robot.
881 
882  \param[in] frame : Control frame in which the force/torque is applied.
883 
884  \param[in] ft : Force/torque vector. The size of this vector
885  is always 6 for a cartesian force/torque skew, and 7 for joint torques.
886 
887  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
888  To diable the filter set filter_gain to 1.
889  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
890  */
892  const double &filter_gain, const bool &activate_pi_controller)
893 {
894  switch (frame) {
895  // Saturation in joint space
896  case JOINT_STATE: {
897  if (ft.size() != 7) {
898  throw vpRobotException(vpRobotException::wrongStateError, "Joint torques vector (%d) is not of size 7",
899  ft.size());
900  }
901 
902  for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
903  m_tau_J_des[i] = ft[i];
904  }
905  // TODO: Introduce force/torque saturation
906 
907  break;
908  }
909 
910  // Saturation in cartesian space
911  case vpRobot::TOOL_FRAME:
914  if (ft.size() != 6) {
915  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian force/torque vector (%d) is not of size 6",
916  ft.size());
917  }
918 
919  m_ft_cart_des = ft;
920  // TODO: Introduce force/torque saturation
921 
922  break;
923  }
924 
925  case vpRobot::MIXT_FRAME: {
926  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
927  }
928  }
929 
930  if (!m_ftControlThreadIsRunning) {
931  getRobotInternalState(); // Update m_robot_state internally
932  m_ftControlThreadIsRunning = true;
933  m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
934  std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
935  std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
936  std::ref(m_mutex), filter_gain, activate_pi_controller);
937  }
938 }
939 
941 {
942  if (!m_handler) {
943  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
944  }
945  franka::RobotState robot_state;
946 
947  if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
948  robot_state = m_handler->readOnce();
949 
950  std::lock_guard<std::mutex> lock(m_mutex);
951  m_robot_state = robot_state;
952  }
953  else { // robot_state is updated in the velocity control thread
954  std::lock_guard<std::mutex> lock(m_mutex);
955  robot_state = m_robot_state;
956  }
957 
958  return robot_state;
959 }
960 
967 {
968  vpColVector q_min(m_q_min.size());
969  for (size_t i = 0; i < m_q_min.size(); i++)
970  q_min[i] = m_q_min[i];
971 
972  return q_min;
973 }
980 {
981  vpColVector q_max(m_q_max.size());
982  for (size_t i = 0; i < m_q_max.size(); i++)
983  q_max[i] = m_q_max[i];
984 
985  return q_max;
986 }
987 
999 
1012 void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc) { m_eMc = eMc; }
1013 
1023 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1024 {
1025  vpColVector q;
1026 
1027  this->readPosFile(filename, q);
1029  this->setPositioningVelocity(velocity_percentage);
1031 }
1032 
1077 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1078 {
1079  std::ifstream fd(filename.c_str(), std::ios::in);
1080 
1081  if (!fd.is_open()) {
1082  return false;
1083  }
1084 
1085  std::string line;
1086  std::string key("R:");
1087  std::string id("#PANDA - Joint position file");
1088  bool pos_found = false;
1089  int lineNum = 0;
1090  size_t njoints = 7;
1091 
1092  q.resize(njoints);
1093 
1094  while (std::getline(fd, line)) {
1095  lineNum++;
1096  if (lineNum == 1) {
1097  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1098  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1099  return false;
1100  }
1101  }
1102  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1103  continue;
1104  }
1105  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1106  // check if there are at least njoint values in the line
1107  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1108  if (chain.size() < njoints + 1) // try to split with tab separator
1109  chain = vpIoTools::splitChain(line, std::string("\t"));
1110  if (chain.size() < njoints + 1)
1111  continue;
1112 
1113  std::istringstream ss(line);
1114  std::string key_;
1115  ss >> key_;
1116  for (unsigned int i = 0; i < njoints; i++)
1117  ss >> q[i];
1118  pos_found = true;
1119  break;
1120  }
1121  }
1122 
1123  // converts rotations from degrees into radians
1124  for (unsigned int i = 0; i < njoints; i++) {
1125  q[i] = vpMath::rad(q[i]);
1126  }
1127 
1128  fd.close();
1129 
1130  if (!pos_found) {
1131  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1132  return false;
1133  }
1134 
1135  return true;
1136 }
1137 
1160 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1161 {
1162 
1163  FILE *fd;
1164  fd = fopen(filename.c_str(), "w");
1165  if (fd == nullptr)
1166  return false;
1167 
1168  fprintf(fd, "#PANDA - Joint position file\n"
1169  "#\n"
1170  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1171  "# with joint positions q1 to q7 expressed in degrees\n"
1172  "#\n");
1173 
1174  // Save positions in mm and deg
1175  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1176  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1177 
1178  fclose(fd);
1179  return (true);
1180 }
1181 
1190 {
1192  vpColVector q(7, 0);
1194  }
1196 }
1197 
1205 {
1206  if (m_franka_address.empty()) {
1207  throw(vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1208  }
1209  if (m_gripper == nullptr)
1210  m_gripper = new franka::Gripper(m_franka_address);
1211 
1212  m_gripper->homing();
1213 }
1214 
1225 {
1226  if (m_franka_address.empty()) {
1227  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1228  }
1229  if (m_gripper == nullptr)
1230  m_gripper = new franka::Gripper(m_franka_address);
1231 
1232  // Check for the maximum grasping width.
1233  franka::GripperState gripper_state = m_gripper->readOnce();
1234 
1235  if (gripper_state.max_width < width) {
1236  std::cout << "Finger width request is too large for the current fingers on the gripper."
1237  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1238  return EXIT_FAILURE;
1239  }
1240 
1241  m_gripper->move(width, 0.1);
1242  return EXIT_SUCCESS;
1243 }
1244 
1254 
1264 {
1265  if (m_franka_address.empty()) {
1266  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1267  }
1268  if (m_gripper == nullptr)
1269  m_gripper = new franka::Gripper(m_franka_address);
1270 
1271  // Check for the maximum grasping width.
1272  franka::GripperState gripper_state = m_gripper->readOnce();
1273 
1274  m_gripper->move(gripper_state.max_width, 0.1);
1275  return EXIT_SUCCESS;
1276 }
1277 
1285 {
1286  if (m_franka_address.empty()) {
1287  throw(vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1288  }
1289  if (m_gripper == nullptr)
1290  m_gripper = new franka::Gripper(m_franka_address);
1291 
1292  m_gripper->stop();
1293 }
1294 
1309 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1310 {
1311  return gripperGrasp(grasping_width, 0.1, force);
1312 }
1313 
1329 int vpRobotFranka::gripperGrasp(double grasping_width, double speed, double force)
1330 {
1331  if (m_gripper == nullptr)
1332  m_gripper = new franka::Gripper(m_franka_address);
1333 
1334  // Check for the maximum grasping width.
1335  franka::GripperState gripper_state = m_gripper->readOnce();
1336  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1337  if (gripper_state.max_width < grasping_width) {
1338  std::cout << "Object is too large for the current fingers on the gripper."
1339  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1340  return EXIT_FAILURE;
1341  }
1342 
1343  // Grasp the object.
1344  if (!m_gripper->grasp(grasping_width, speed, force)) {
1345  std::cout << "Failed to grasp object." << std::endl;
1346  return EXIT_FAILURE;
1347  }
1348 
1349  return EXIT_SUCCESS;
1350 }
1351 
1352 #elif !defined(VISP_BUILD_SHARED_LIBS)
1353 // Work around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1354 // no symbols
1355 void dummy_vpRobotFranka() { };
1356 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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:2372
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:818
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:967
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.)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
void getGravity(vpColVector &gravity)
int gripperMove(double width)
void getMass(vpMatrix &mass)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_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 setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
void setPositioningVelocity(double velocity)
void get_eJe(vpMatrix &eJe) vp_override
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
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)
void get_fJe(vpMatrix &fJe) vp_override
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