Visual Servoing Platform  version 3.6.1 under development (2024-07-23)
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 
55  : vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
56  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
57  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
58  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(),
59  m_franka_address()
60 {
61  init();
62 }
63 
70 vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
71  : vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
72  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
73  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
74  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(),
75  m_franka_address()
76 {
77  init();
78  connect(franka_address, realtime_config);
79 }
80 
84 void vpRobotFranka::init()
85 {
86  nDof = 7;
87 
88  m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
89  m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
90  m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
91  m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
92 }
93 
100 {
102 
103  if (m_handler)
104  delete m_handler;
105 
106  if (m_gripper) {
107  std::cout << "Grasped object, will release it now." << std::endl;
108  m_gripper->stop();
109  delete m_gripper;
110  }
111 
112  if (m_model) {
113  delete m_model;
114  }
115 }
116 
123 void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
124 {
125  init();
126  if (franka_address.empty()) {
127  throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
128  }
129  if (m_handler)
130  delete m_handler;
131 
132  m_franka_address = franka_address;
133  m_handler = new franka::Robot(franka_address, realtime_config);
134 
135  std::array<double, 7> lower_torque_thresholds_nominal { {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.} };
136  std::array<double, 7> upper_torque_thresholds_nominal { {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0} };
137  std::array<double, 7> lower_torque_thresholds_acceleration { {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0} };
138  std::array<double, 7> upper_torque_thresholds_acceleration { {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0} };
139  std::array<double, 6> lower_force_thresholds_nominal { {30.0, 30.0, 30.0, 25.0, 25.0, 25.0} };
140  std::array<double, 6> upper_force_thresholds_nominal { {40.0, 40.0, 40.0, 35.0, 35.0, 35.0} };
141  std::array<double, 6> lower_force_thresholds_acceleration { {30.0, 30.0, 30.0, 25.0, 25.0, 25.0} };
142  std::array<double, 6> upper_force_thresholds_acceleration { {40.0, 40.0, 40.0, 35.0, 35.0, 35.0} };
143  m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
144  lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
145  lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
146  lower_force_thresholds_nominal, upper_force_thresholds_nominal);
147 
148  m_handler->setJointImpedance({ {3000, 3000, 3000, 2500, 2500, 2000, 2000} });
149  m_handler->setCartesianImpedance({ {3000, 3000, 3000, 300, 300, 300} });
150 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
151  // m_handler->setFilters(100, 100, 100, 100, 100);
152  m_handler->setFilters(10, 10, 10, 10, 10);
153 #else
154  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
155 #endif
156  if (m_model) {
157  delete m_model;
158  }
159  m_model = new franka::Model(m_handler->loadModel());
160 }
161 
177 {
178  if (!m_handler) {
179  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
180  }
181 
182  franka::RobotState robot_state = getRobotInternalState();
183  vpColVector q(7);
184  for (int i = 0; i < 7; i++) {
185  q[i] = robot_state.q[i];
186  }
187 
188  switch (frame) {
189  case JOINT_STATE: {
190  position = q;
191  break;
192  }
193  case END_EFFECTOR_FRAME: {
194  position.resize(6);
195  vpHomogeneousMatrix fMe = get_fMe(q);
196  vpPoseVector fPe(fMe);
197  for (size_t i = 0; i < 6; i++) {
198  position[i] = fPe[i];
199  }
200  break;
201  }
202  case TOOL_FRAME: { // same a CAMERA_FRAME
203  position.resize(6);
204  vpHomogeneousMatrix fMc = get_fMc(q);
205  vpPoseVector fPc(fMc);
206  for (size_t i = 0; i < 6; i++) {
207  position[i] = fPc[i];
208  }
209  break;
210  }
211  default: {
212  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
213  }
214  }
215 }
216 
228 {
229  if (!m_handler) {
230  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
231  }
232 
233  franka::RobotState robot_state = getRobotInternalState();
234 
235  switch (frame) {
236  case JOINT_STATE: {
237  force.resize(7);
238  for (int i = 0; i < 7; i++)
239  force[i] = robot_state.tau_J[i];
240 
241  break;
242  }
243  case END_EFFECTOR_FRAME: {
244  force.resize(6);
245  for (int i = 0; i < 6; i++)
246  force[i] = robot_state.K_F_ext_hat_K[i];
247  break;
248  }
249  case TOOL_FRAME: {
250  // end-effector frame
251  vpColVector eFe(6);
252  for (int i = 0; i < 6; i++)
253  eFe[i] = robot_state.K_F_ext_hat_K[i];
254 
255  // Transform in tool frame
257  vpForceTwistMatrix cWe(cMe);
258  force = cWe * eFe;
259  break;
260  }
261  default: {
262  throw(vpException(vpException::fatalError, "Cannot get Franka force/torque: frame not supported"));
263  }
264  }
265 }
266 
282 {
283  if (!m_handler) {
284  throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
285  }
286 
287  franka::RobotState robot_state = getRobotInternalState();
288 
289  switch (frame) {
290 
291  case JOINT_STATE: {
292  d_position.resize(7);
293  for (int i = 0; i < 7; i++) {
294  d_position[i] = robot_state.dq[i];
295  }
296  break;
297  }
298 
299  default: {
300  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
301  }
302  }
303 }
304 
311 {
312  if (!m_handler) {
313  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
314  }
315 
316  std::array<double, 7> coriolis_;
317 
318  franka::RobotState robot_state = getRobotInternalState();
319 
320  coriolis_ = m_model->coriolis(robot_state);
321 
322  coriolis.resize(7);
323  for (int i = 0; i < 7; i++) {
324  coriolis[i] = coriolis_[i];
325  }
326 }
327 
333 {
334  if (!m_handler) {
335  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
336  }
337 
338  std::array<double, 7> gravity_;
339 
340  franka::RobotState robot_state = getRobotInternalState();
341 
342  gravity_ = m_model->gravity(robot_state);
343 
344  gravity.resize(7);
345  for (int i = 0; i < 7; i++) {
346  gravity[i] = gravity_[i];
347  }
348 }
349 
355 {
356  if (!m_handler) {
357  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
358  }
359 
360  std::array<double, 49> mass_;
361 
362  franka::RobotState robot_state = getRobotInternalState();
363 
364  mass_ = m_model->mass(robot_state); // column-major
365 
366  mass.resize(7, 7); // row-major
367  for (size_t i = 0; i < 7; i++) {
368  for (size_t j = 0; j < 7; j++) {
369  mass[i][j] = mass_[j * 7 + i];
370  }
371  }
372 }
373 
382 {
383  if (!m_handler) {
384  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
385  }
386  if (q.size() != 7) {
387  throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
388  }
389 
390  std::array<double, 7> q_array;
391  for (size_t i = 0; i < 7; i++)
392  q_array[i] = q[i];
393 
394  franka::RobotState robot_state = getRobotInternalState();
395 
396  std::array<double, 16> pose_array =
397  m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
399  for (unsigned int i = 0; i < 4; i++) {
400  for (unsigned int j = 0; j < 4; j++) {
401  fMe[i][j] = pose_array[j * 4 + i];
402  }
403  }
404 
405  return fMe;
406 }
407 
423 {
424  vpHomogeneousMatrix fMe = get_fMe(q);
425  return (fMe * m_eMc);
426 }
427 
438 {
439  if (!m_handler) {
440  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
441  }
442  if (frame == JOINT_STATE) {
443  throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
444  }
445 
446  franka::RobotState robot_state = getRobotInternalState();
447 
448  std::array<double, 16> pose_array = robot_state.O_T_EE;
450  for (unsigned int i = 0; i < 4; i++) {
451  for (unsigned int j = 0; j < 4; j++) {
452  fMe[i][j] = pose_array[j * 4 + i];
453  }
454  }
455 
456  switch (frame) {
457  case END_EFFECTOR_FRAME: {
458  pose.build(fMe);
459  break;
460  }
461  case TOOL_FRAME: {
462  pose.build(fMe * m_eMc);
463  break;
464  }
465  default: {
466  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
467  }
468  }
469 }
470 
477 {
478  if (!m_handler) {
479  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
480  }
481 
482  franka::RobotState robot_state = getRobotInternalState();
483 
484  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
485  eJe_.resize(6, 7); // row-major
486  for (size_t i = 0; i < 6; i++) {
487  for (size_t j = 0; j < 7; j++) {
488  eJe_[i][j] = jacobian[j * 6 + i];
489  }
490  }
491  // TODO check from vpRobot fJe and fJeAvailable
492 }
493 
500 {
501  if (!m_handler) {
502  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
503  }
504 
505  franka::RobotState robot_state = getRobotInternalState();
506 
507  std::array<double, 7> q_array;
508  for (size_t i = 0; i < 7; i++)
509  q_array[i] = q[i];
510 
511  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
512  robot_state.EE_T_K); // column-major
513  eJe_.resize(6, 7); // row-major
514  for (size_t i = 0; i < 6; i++) {
515  for (size_t j = 0; j < 7; j++) {
516  eJe_[i][j] = jacobian[j * 6 + i];
517  }
518  }
519  // TODO check from vpRobot fJe and fJeAvailable
520 }
521 
528 {
529  if (!m_handler) {
530  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
531  }
532 
533  franka::RobotState robot_state = getRobotInternalState();
534 
535  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
536  fJe_.resize(6, 7); // row-major
537  for (size_t i = 0; i < 6; i++) {
538  for (size_t j = 0; j < 7; j++) {
539  fJe_[i][j] = jacobian[j * 6 + i];
540  }
541  }
542  // TODO check from vpRobot fJe and fJeAvailable
543 }
544 
551 {
552  if (!m_handler) {
553  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
554  }
555  if (q.size() != 7) {
556  throw(vpException(
558  "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
559  q.size()));
560  }
561 
562  franka::RobotState robot_state = getRobotInternalState();
563 
564  std::array<double, 7> q_array;
565  for (size_t i = 0; i < 7; i++)
566  q_array[i] = q[i];
567 
568  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
569  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"
621  << std::endl;
623  }
624 
625  if (frame == vpRobot::JOINT_STATE) {
626  double speed_factor = m_positioningVelocity / 100.;
627 
628  std::array<double, 7> q_goal;
629  for (size_t i = 0; i < 7; i++) {
630  q_goal[i] = position[i];
631  }
632 
633  vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
634 
635  int nbAttempts = 10;
636  for (int attempt = 1; attempt <= nbAttempts; attempt++) {
637  try {
638  m_handler->control(joint_pos_traj_generator);
639  break;
640  }
641  catch (const franka::ControlException &e) {
642  std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
643  m_handler->automaticErrorRecovery();
644  if (attempt == nbAttempts)
645  throw;
646  }
647  }
648  }
649  else {
651  "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
652  }
653 }
654 
663 void vpRobotFranka::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
664 
672 {
673  switch (newState) {
674  case vpRobot::STATE_STOP: {
675  // Start primitive STOP only if the current state is velocity or force/torque
677  // Stop the robot
678  m_velControlThreadStopAsked = true;
679  if (m_velControlThread.joinable()) {
680  m_velControlThread.join();
681  m_velControlThreadStopAsked = false;
682  m_velControlThreadIsRunning = false;
683  }
684  }
686  // Stop the robot
687  m_ftControlThreadStopAsked = true;
688  if (m_ftControlThread.joinable()) {
689  m_ftControlThread.join();
690  m_ftControlThreadStopAsked = false;
691  m_ftControlThreadIsRunning = false;
692  }
693  }
694  break;
695  }
698  std::cout << "Change the control mode from velocity to position control.\n";
699  // Stop the robot
700  m_velControlThreadStopAsked = true;
701  if (m_velControlThread.joinable()) {
702  m_velControlThread.join();
703  m_velControlThreadStopAsked = false;
704  m_velControlThreadIsRunning = false;
705  }
706  }
708  std::cout << "Change the control mode from force/torque to position control.\n";
709  // Stop the robot
710  m_ftControlThreadStopAsked = true;
711  if (m_ftControlThread.joinable()) {
712  m_ftControlThread.join();
713  m_ftControlThreadStopAsked = false;
714  m_ftControlThreadIsRunning = false;
715  }
716  }
717  break;
718  }
721  std::cout << "Change the control mode from stop to velocity control.\n";
722  }
724  std::cout << "Change the control mode from position to velocity control.\n";
725  }
727  std::cout << "Change the control mode from force/torque to velocity control.\n";
728  // Stop the robot
729  m_ftControlThreadStopAsked = true;
730  if (m_ftControlThread.joinable()) {
731  m_ftControlThread.join();
732  m_ftControlThreadStopAsked = false;
733  m_ftControlThreadIsRunning = false;
734  }
735  }
736  break;
737  }
740  std::cout << "Change the control mode from stop to force/torque control.\n";
741  }
743  std::cout << "Change the control mode from position to force/torque control.\n";
744  }
746  std::cout << "Change the control mode from velocity to force/torque control.\n";
747  // Stop the robot
748  m_velControlThreadStopAsked = true;
749  if (m_velControlThread.joinable()) {
750  m_velControlThread.join();
751  m_velControlThreadStopAsked = false;
752  m_velControlThreadIsRunning = false;
753  }
754  }
755  break;
756  }
757 
758  default:
759  break;
760  }
761 
762  return vpRobot::setRobotState(newState);
763 }
764 
819 {
822  "Cannot send a velocity to the robot. "
823  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
824  }
825 
826  switch (frame) {
827  // Saturation in joint space
828  case JOINT_STATE: {
829  if (vel.size() != 7) {
830  throw vpRobotException(vpRobotException::wrongStateError, "Joint velocity vector (%d) is not of size 7",
831  vel.size());
832  }
833 
834  vpColVector vel_max(7, getMaxRotationVelocity());
835 
836  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
837 
838  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
839  m_dq_des[i] = vel_sat[i];
840  }
841 
842  break;
843  }
844 
845  // Saturation in cartesian space
846  case vpRobot::TOOL_FRAME:
849  if (vel.size() != 6) {
850  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian velocity vector (%d) is not of size 6",
851  vel.size());
852  }
853  vpColVector vel_max(6);
854 
855  for (unsigned int i = 0; i < 3; i++)
856  vel_max[i] = getMaxTranslationVelocity();
857  for (unsigned int i = 3; i < 6; i++)
858  vel_max[i] = getMaxRotationVelocity();
859 
860  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
861 
862  break;
863  }
864 
865  case vpRobot::MIXT_FRAME: {
866  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
867  }
868  }
869 
870  if (!m_velControlThreadIsRunning) {
871  m_velControlThreadIsRunning = true;
872  m_velControlThread =
873  std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
874  std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
875  std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
876  std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
877  }
878 }
879 
880 /*
881  Apply a force/torque to the robot.
882 
883  \param[in] frame : Control frame in which the force/torque is applied.
884 
885  \param[in] ft : Force/torque vector. The size of this vector
886  is always 6 for a cartesian force/torque skew, and 7 for joint torques.
887 
888  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
889  To diable the filter set filter_gain to 1.
890  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
891  */
893  const double &filter_gain, const bool &activate_pi_controller)
894 {
895  switch (frame) {
896  // Saturation in joint space
897  case JOINT_STATE: {
898  if (ft.size() != 7) {
899  throw vpRobotException(vpRobotException::wrongStateError, "Joint torques vector (%d) is not of size 7",
900  ft.size());
901  }
902 
903  for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
904  m_tau_J_des[i] = ft[i];
905  }
906  // TODO: Introduce force/torque saturation
907 
908  break;
909  }
910 
911  // Saturation in cartesian space
912  case vpRobot::TOOL_FRAME:
915  if (ft.size() != 6) {
916  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian force/torque vector (%d) is not of size 6",
917  ft.size());
918  }
919 
920  m_ft_cart_des = ft;
921  // TODO: Introduce force/torque saturation
922 
923  break;
924  }
925 
926  case vpRobot::MIXT_FRAME: {
927  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
928  }
929  }
930 
931  if (!m_ftControlThreadIsRunning) {
932  getRobotInternalState(); // Update m_robot_state internally
933  m_ftControlThreadIsRunning = true;
934  m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
935  std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
936  std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
937  std::ref(m_mutex), filter_gain, activate_pi_controller);
938  }
939 }
940 
942 {
943  if (!m_handler) {
944  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
945  }
946  franka::RobotState robot_state;
947 
948  if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
949  robot_state = m_handler->readOnce();
950 
951  std::lock_guard<std::mutex> lock(m_mutex);
952  m_robot_state = robot_state;
953  }
954  else { // robot_state is updated in the velocity control thread
955  std::lock_guard<std::mutex> lock(m_mutex);
956  robot_state = m_robot_state;
957  }
958 
959  return robot_state;
960 }
961 
968 {
969  vpColVector q_min(m_q_min.size());
970  for (size_t i = 0; i < m_q_min.size(); i++)
971  q_min[i] = m_q_min[i];
972 
973  return q_min;
974 }
981 {
982  vpColVector q_max(m_q_max.size());
983  for (size_t i = 0; i < m_q_max.size(); i++)
984  q_max[i] = m_q_max[i];
985 
986  return q_max;
987 }
988 
1000 
1013 void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc) { m_eMc = eMc; }
1014 
1024 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1025 {
1026  vpColVector q;
1027 
1028  this->readPosFile(filename, q);
1030  this->setPositioningVelocity(velocity_percentage);
1032 }
1033 
1078 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1079 {
1080  std::ifstream fd(filename.c_str(), std::ios::in);
1081 
1082  if (!fd.is_open()) {
1083  return false;
1084  }
1085 
1086  std::string line;
1087  std::string key("R:");
1088  std::string id("#PANDA - Joint position file");
1089  bool pos_found = false;
1090  int lineNum = 0;
1091  size_t njoints = 7;
1092 
1093  q.resize(njoints);
1094 
1095  while (std::getline(fd, line)) {
1096  lineNum++;
1097  if (lineNum == 1) {
1098  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1099  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1100  return false;
1101  }
1102  }
1103  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1104  continue;
1105  }
1106  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1107  // check if there are at least njoint values in the line
1108  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1109  if (chain.size() < njoints + 1) // try to split with tab separator
1110  chain = vpIoTools::splitChain(line, std::string("\t"));
1111  if (chain.size() < njoints + 1)
1112  continue;
1113 
1114  std::istringstream ss(line);
1115  std::string key_;
1116  ss >> key_;
1117  for (unsigned int i = 0; i < njoints; i++)
1118  ss >> q[i];
1119  pos_found = true;
1120  break;
1121  }
1122  }
1123 
1124  // converts rotations from degrees into radians
1125  for (unsigned int i = 0; i < njoints; i++) {
1126  q[i] = vpMath::rad(q[i]);
1127  }
1128 
1129  fd.close();
1130 
1131  if (!pos_found) {
1132  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1133  return false;
1134  }
1135 
1136  return true;
1137 }
1138 
1161 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1162 {
1163 
1164  FILE *fd;
1165  fd = fopen(filename.c_str(), "w");
1166  if (fd == nullptr)
1167  return false;
1168 
1169  fprintf(fd, "#PANDA - Joint position file\n"
1170  "#\n"
1171  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1172  "# with joint positions q1 to q7 expressed in degrees\n"
1173  "#\n");
1174 
1175  // Save positions in mm and deg
1176  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1177  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1178 
1179  fclose(fd);
1180  return (true);
1181 }
1182 
1191 {
1193  vpColVector q(7, 0);
1195  }
1197 }
1198 
1206 {
1207  if (m_franka_address.empty()) {
1208  throw(vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1209  }
1210  if (m_gripper == nullptr)
1211  m_gripper = new franka::Gripper(m_franka_address);
1212 
1213  m_gripper->homing();
1214 }
1215 
1226 {
1227  if (m_franka_address.empty()) {
1228  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1229  }
1230  if (m_gripper == nullptr)
1231  m_gripper = new franka::Gripper(m_franka_address);
1232 
1233  // Check for the maximum grasping width.
1234  franka::GripperState gripper_state = m_gripper->readOnce();
1235 
1236  if (gripper_state.max_width < width) {
1237  std::cout << "Finger width request is too large for the current fingers on the gripper."
1238  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1239  return EXIT_FAILURE;
1240  }
1241 
1242  m_gripper->move(width, 0.1);
1243  return EXIT_SUCCESS;
1244 }
1245 
1255 
1265 {
1266  if (m_franka_address.empty()) {
1267  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1268  }
1269  if (m_gripper == nullptr)
1270  m_gripper = new franka::Gripper(m_franka_address);
1271 
1272  // Check for the maximum grasping width.
1273  franka::GripperState gripper_state = m_gripper->readOnce();
1274 
1275  m_gripper->move(gripper_state.max_width, 0.1);
1276  return EXIT_SUCCESS;
1277 }
1278 
1286 {
1287  if (m_franka_address.empty()) {
1288  throw(vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1289  }
1290  if (m_gripper == nullptr)
1291  m_gripper = new franka::Gripper(m_franka_address);
1292 
1293  m_gripper->stop();
1294 }
1295 
1310 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1311 {
1312  return gripperGrasp(grasping_width, 0.1, force);
1313 }
1314 
1330 int vpRobotFranka::gripperGrasp(double grasping_width, double speed, double force)
1331 {
1332  if (m_gripper == nullptr)
1333  m_gripper = new franka::Gripper(m_franka_address);
1334 
1335  // Check for the maximum grasping width.
1336  franka::GripperState gripper_state = m_gripper->readOnce();
1337  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1338  if (gripper_state.max_width < grasping_width) {
1339  std::cout << "Object is too large for the current fingers on the gripper."
1340  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1341  return EXIT_FAILURE;
1342  }
1343 
1344  // Grasp the object.
1345  if (!m_gripper->grasp(grasping_width, speed, force)) {
1346  std::cout << "Failed to grasp object." << std::endl;
1347  return EXIT_FAILURE;
1348  }
1349 
1350  return EXIT_SUCCESS;
1351 }
1352 END_VISP_NAMESPACE
1353 #elif !defined(VISP_BUILD_SHARED_LIBS)
1354 // Work around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1355 // no symbols
1356 void dummy_vpRobotFranka() { };
1357 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:66
@ fatalError
Fatal error.
Definition: vpException.h:72
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:1661
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:396
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:550
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
vpPoseVector & build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
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 getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) 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 setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe) VP_OVERRIDE
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_eJe(vpMatrix &eJe) VP_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)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) VP_OVERRIDE
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
int nDof
number of degrees of freedom
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:155
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:164
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ JOINT_STATE
Definition: vpRobot.h:82
@ TOOL_FRAME
Definition: vpRobot.h:86
@ MIXT_FRAME
Definition: vpRobot.h:88
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
@ STATE_FORCE_TORQUE_CONTROL
Initialize the force/torque controller.
Definition: vpRobot.h:70
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:274
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252