Visual Servoing Platform  version 3.5.1 under development (2022-12-04)
vpRobotFranka.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See 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  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_FRANKA
42 
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotException.h>
45 #include <visp3/robot/vpRobotFranka.h>
46 
47 #include "vpForceTorqueGenerator_impl.h"
48 #include "vpJointPosTrajGenerator_impl.h"
49 #include "vpJointVelTrajGenerator_impl.h"
50 
57  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_velControlThread(),
58  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
59  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
60  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(),
61  m_franka_address()
62 {
63  init();
64 }
65 
72 vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
73  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_velControlThread(),
74  m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
75  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
76  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(),
77  m_franka_address()
78 {
79  init();
80  connect(franka_address, realtime_config);
81 }
82 
86 void vpRobotFranka::init()
87 {
88  nDof = 7;
89 
90  m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
91  m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
92  m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
93  m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
94 }
95 
102 {
104 
105  if (m_handler)
106  delete m_handler;
107 
108  if (m_gripper) {
109  std::cout << "Grasped object, will release it now." << std::endl;
110  m_gripper->stop();
111  delete m_gripper;
112  }
113 
114  if (m_model) {
115  delete m_model;
116  }
117 }
118 
125 void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
126 {
127  init();
128  if (franka_address.empty()) {
129  throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
130  }
131  if (m_handler)
132  delete m_handler;
133 
134  m_franka_address = franka_address;
135  m_handler = new franka::Robot(franka_address, realtime_config);
136 
137  std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
138  std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
139  std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
140  std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
141  std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
142  std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
143  std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
144  std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
145  m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
146  lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
147  lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
148  lower_force_thresholds_nominal, upper_force_thresholds_nominal);
149 
150  m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
151  m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
152 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
153  // m_handler->setFilters(100, 100, 100, 100, 100);
154  m_handler->setFilters(10, 10, 10, 10, 10);
155 #else
156  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
157 #endif
158  if (m_model) {
159  delete m_model;
160  }
161  m_model = new franka::Model(m_handler->loadModel());
162 }
163 
179 {
180  if (!m_handler) {
181  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
182  }
183 
184  franka::RobotState robot_state = getRobotInternalState();
185  vpColVector q(7);
186  for (int i = 0; i < 7; i++) {
187  q[i] = robot_state.q[i];
188  }
189 
190  switch (frame) {
191  case JOINT_STATE: {
192  position = q;
193  break;
194  }
195  case END_EFFECTOR_FRAME: {
196  position.resize(6);
197  vpHomogeneousMatrix fMe = get_fMe(q);
198  vpPoseVector fPe(fMe);
199  for (size_t i = 0; i < 6; i++) {
200  position[i] = fPe[i];
201  }
202  break;
203  }
204  case TOOL_FRAME: { // same a CAMERA_FRAME
205  position.resize(6);
206  vpHomogeneousMatrix fMc = get_fMc(q);
207  vpPoseVector fPc(fMc);
208  for (size_t i = 0; i < 6; i++) {
209  position[i] = fPc[i];
210  }
211  break;
212  }
213  default: {
214  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
215  }
216  }
217 }
218 
230 {
231  if (!m_handler) {
232  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
233  }
234 
235  franka::RobotState robot_state = getRobotInternalState();
236 
237  switch (frame) {
238  case JOINT_STATE: {
239  force.resize(7);
240  for (int i = 0; i < 7; i++)
241  force[i] = robot_state.tau_J[i];
242 
243  break;
244  }
245  case END_EFFECTOR_FRAME: {
246  force.resize(6);
247  for (int i = 0; i < 6; i++)
248  force[i] = robot_state.K_F_ext_hat_K[i];
249  break;
250  }
251  case TOOL_FRAME: {
252  // end-effector frame
253  vpColVector eFe(6);
254  for (int i = 0; i < 6; i++)
255  eFe[i] = robot_state.K_F_ext_hat_K[i];
256 
257  // Transform in tool frame
259  vpForceTwistMatrix cWe(cMe);
260  force = cWe * eFe;
261  break;
262  }
263  default: {
264  throw(vpException(vpException::fatalError, "Cannot get Franka force/torque: frame not supported"));
265  }
266  }
267 }
268 
284 {
285  if (!m_handler) {
286  throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
287  }
288 
289  franka::RobotState robot_state = getRobotInternalState();
290 
291  switch (frame) {
292 
293  case JOINT_STATE: {
294  d_position.resize(7);
295  for (int i = 0; i < 7; i++) {
296  d_position[i] = robot_state.dq[i];
297  }
298  break;
299  }
300 
301  default: {
302  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
303  }
304  }
305 }
306 
313 {
314  if (!m_handler) {
315  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
316  }
317 
318  std::array<double, 7> coriolis_;
319 
320  franka::RobotState robot_state = getRobotInternalState();
321 
322  coriolis_ = m_model->coriolis(robot_state);
323 
324  coriolis.resize(7);
325  for (int i = 0; i < 7; i++) {
326  coriolis[i] = coriolis_[i];
327  }
328 }
329 
335 {
336  if (!m_handler) {
337  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
338  }
339 
340  std::array<double, 7> gravity_;
341 
342  franka::RobotState robot_state = getRobotInternalState();
343 
344  gravity_ = m_model->gravity(robot_state);
345 
346  gravity.resize(7);
347  for (int i = 0; i < 7; i++) {
348  gravity[i] = gravity_[i];
349  }
350 }
351 
357 {
358  if (!m_handler) {
359  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
360  }
361 
362  std::array<double, 49> mass_;
363 
364  franka::RobotState robot_state = getRobotInternalState();
365 
366  mass_ = m_model->mass(robot_state); // column-major
367 
368  mass.resize(7, 7); // row-major
369  for (size_t i = 0; i < 7; i++) {
370  for (size_t j = 0; j < 7; j++) {
371  mass[i][j] = mass_[j * 7 + i];
372  }
373  }
374 }
375 
384 {
385  if (!m_handler) {
386  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
387  }
388  if (q.size() != 7) {
389  throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
390  }
391 
392  std::array<double, 7> q_array;
393  for (size_t i = 0; i < 7; i++)
394  q_array[i] = q[i];
395 
396  franka::RobotState robot_state = getRobotInternalState();
397 
398  std::array<double, 16> pose_array =
399  m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
401  for (unsigned int i = 0; i < 4; i++) {
402  for (unsigned int j = 0; j < 4; j++) {
403  fMe[i][j] = pose_array[j * 4 + i];
404  }
405  }
406 
407  return fMe;
408 }
409 
425 {
426  vpHomogeneousMatrix fMe = get_fMe(q);
427  return (fMe * m_eMc);
428 }
429 
440 {
441  if (!m_handler) {
442  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
443  }
444  if (frame == JOINT_STATE) {
445  throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
446  }
447 
448  franka::RobotState robot_state = getRobotInternalState();
449 
450  std::array<double, 16> pose_array = robot_state.O_T_EE;
452  for (unsigned int i = 0; i < 4; i++) {
453  for (unsigned int j = 0; j < 4; j++) {
454  fMe[i][j] = pose_array[j * 4 + i];
455  }
456  }
457 
458  switch (frame) {
459  case END_EFFECTOR_FRAME: {
460  pose.buildFrom(fMe);
461  break;
462  }
463  case TOOL_FRAME: {
464  pose.buildFrom(fMe * m_eMc);
465  break;
466  }
467  default: {
468  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
469  }
470  }
471 }
472 
479 {
480  if (!m_handler) {
481  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
482  }
483 
484  franka::RobotState robot_state = getRobotInternalState();
485 
486  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
487  eJe_.resize(6, 7); // row-major
488  for (size_t i = 0; i < 6; i++) {
489  for (size_t j = 0; j < 7; j++) {
490  eJe_[i][j] = jacobian[j * 6 + i];
491  }
492  }
493  // TODO check from vpRobot fJe and fJeAvailable
494 }
495 
502 {
503  if (!m_handler) {
504  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
505  }
506 
507  franka::RobotState robot_state = getRobotInternalState();
508 
509  std::array<double, 7> q_array;
510  for (size_t i = 0; i < 7; i++)
511  q_array[i] = q[i];
512 
513  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
514  robot_state.EE_T_K); // column-major
515  eJe_.resize(6, 7); // row-major
516  for (size_t i = 0; i < 6; i++) {
517  for (size_t j = 0; j < 7; j++) {
518  eJe_[i][j] = jacobian[j * 6 + i];
519  }
520  }
521  // TODO check from vpRobot fJe and fJeAvailable
522 }
523 
530 {
531  if (!m_handler) {
532  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
533  }
534 
535  franka::RobotState robot_state = getRobotInternalState();
536 
537  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
538  fJe_.resize(6, 7); // row-major
539  for (size_t i = 0; i < 6; i++) {
540  for (size_t j = 0; j < 7; j++) {
541  fJe_[i][j] = jacobian[j * 6 + i];
542  }
543  }
544  // TODO check from vpRobot fJe and fJeAvailable
545 }
546 
553 {
554  if (!m_handler) {
555  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
556  }
557  if (q.size() != 7) {
558  throw(vpException(
560  "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
561  q.size()));
562  }
563 
564  franka::RobotState robot_state = getRobotInternalState();
565 
566  std::array<double, 7> q_array;
567  for (size_t i = 0; i < 7; i++)
568  q_array[i] = q[i];
569 
570  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
571  robot_state.EE_T_K); // column-major
572  fJe_.resize(6, 7); // row-major
573  for (size_t i = 0; i < 6; i++) {
574  for (size_t j = 0; j < 7; j++) {
575  fJe_[i][j] = jacobian[j * 6 + i];
576  }
577  }
578  // TODO check from vpRobot fJe and fJeAvailable
579 }
580 
590 void vpRobotFranka::setLogFolder(const std::string &folder)
591 {
592  if (!folder.empty()) {
593  if (vpIoTools::checkDirectory(folder) == false) {
594  try {
595  vpIoTools::makeDirectory(folder);
596  m_log_folder = folder;
597  } catch (const vpException &e) {
598  std::string error;
599  error = "Unable to create Franka log folder: " + folder;
600  throw(vpException(vpException::fatalError, error));
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_positionningVelocity / 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  } 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  } else {
649  "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
650  }
651 }
652 
661 void vpRobotFranka::setPositioningVelocity(double velocity) { m_positionningVelocity = velocity; }
662 
670 {
671  switch (newState) {
672  case vpRobot::STATE_STOP: {
673  // Start primitive STOP only if the current state is velocity or force/torque
675  // Stop the robot
676  m_velControlThreadStopAsked = true;
677  if (m_velControlThread.joinable()) {
678  m_velControlThread.join();
679  m_velControlThreadStopAsked = false;
680  m_velControlThreadIsRunning = false;
681  }
683  // Stop the robot
684  m_ftControlThreadStopAsked = true;
685  if (m_ftControlThread.joinable()) {
686  m_ftControlThread.join();
687  m_ftControlThreadStopAsked = false;
688  m_ftControlThreadIsRunning = false;
689  }
690  }
691  break;
692  }
695  std::cout << "Change the control mode from velocity to position control.\n";
696  // Stop the robot
697  m_velControlThreadStopAsked = true;
698  if (m_velControlThread.joinable()) {
699  m_velControlThread.join();
700  m_velControlThreadStopAsked = false;
701  m_velControlThreadIsRunning = false;
702  }
704  std::cout << "Change the control mode from force/torque to position control.\n";
705  // Stop the robot
706  m_ftControlThreadStopAsked = true;
707  if (m_ftControlThread.joinable()) {
708  m_ftControlThread.join();
709  m_ftControlThreadStopAsked = false;
710  m_ftControlThreadIsRunning = false;
711  }
712  }
713  break;
714  }
717  std::cout << "Change the control mode from stop to velocity control.\n";
719  std::cout << "Change the control mode from position to velocity control.\n";
721  std::cout << "Change the control mode from force/torque to velocity control.\n";
722  // Stop the robot
723  m_ftControlThreadStopAsked = true;
724  if (m_ftControlThread.joinable()) {
725  m_ftControlThread.join();
726  m_ftControlThreadStopAsked = false;
727  m_ftControlThreadIsRunning = false;
728  }
729  }
730  break;
731  }
734  std::cout << "Change the control mode from stop to force/torque control.\n";
736  std::cout << "Change the control mode from position to force/torque control.\n";
738  std::cout << "Change the control mode from velocity to force/torque control.\n";
739  // Stop the robot
740  m_velControlThreadStopAsked = true;
741  if (m_velControlThread.joinable()) {
742  m_velControlThread.join();
743  m_velControlThreadStopAsked = false;
744  m_velControlThreadIsRunning = false;
745  }
746  }
747  break;
748  }
749 
750  default:
751  break;
752  }
753 
754  return vpRobot::setRobotState(newState);
755 }
756 
811 {
814  "Cannot send a velocity to the robot. "
815  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
816  }
817 
818  switch (frame) {
819  // Saturation in joint space
820  case JOINT_STATE: {
821  if (vel.size() != 7) {
822  throw vpRobotException(vpRobotException::wrongStateError, "Joint velocity vector (%d) is not of size 7",
823  vel.size());
824  }
825 
826  vpColVector vel_max(7, getMaxRotationVelocity());
827 
828  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
829 
830  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
831  m_dq_des[i] = vel_sat[i];
832  }
833 
834  break;
835  }
836 
837  // Saturation in cartesian space
838  case vpRobot::TOOL_FRAME:
841  if (vel.size() != 6) {
842  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian velocity vector (%d) is not of size 6",
843  vel.size());
844  }
845  vpColVector vel_max(6);
846 
847  for (unsigned int i = 0; i < 3; i++)
848  vel_max[i] = getMaxTranslationVelocity();
849  for (unsigned int i = 3; i < 6; i++)
850  vel_max[i] = getMaxRotationVelocity();
851 
852  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
853 
854  break;
855  }
856 
857  case vpRobot::MIXT_FRAME: {
858  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
859  }
860  }
861 
862  if (!m_velControlThreadIsRunning) {
863  m_velControlThreadIsRunning = true;
864  m_velControlThread =
865  std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
866  std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
867  std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
868  std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
869  }
870 }
871 
872 /*
873  Apply a force/torque to the robot.
874 
875  \param[in] frame : Control frame in which the force/torque is applied.
876 
877  \param[in] ft : Force/torque vector. The size of this vector
878  is always 6 for a cartesian force/torque skew, and 7 for joint torques.
879 
880  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
881  To diable the filter set filter_gain to 1.
882  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
883  */
885  const double &filter_gain, const bool &activate_pi_controller)
886 {
887  switch (frame) {
888  // Saturation in joint space
889  case JOINT_STATE: {
890  if (ft.size() != 7) {
891  throw vpRobotException(vpRobotException::wrongStateError, "Joint torques vector (%d) is not of size 7",
892  ft.size());
893  }
894 
895  for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
896  m_tau_J_des[i] = ft[i];
897  }
898  // TODO: Introduce force/torque saturation
899 
900  break;
901  }
902 
903  // Saturation in cartesian space
904  case vpRobot::TOOL_FRAME:
907  if (ft.size() != 6) {
908  throw vpRobotException(vpRobotException::wrongStateError, "Cartesian force/torque vector (%d) is not of size 6",
909  ft.size());
910  }
911 
912  m_ft_cart_des = ft;
913  // TODO: Introduce force/torque saturation
914 
915  break;
916  }
917 
918  case vpRobot::MIXT_FRAME: {
919  throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
920  }
921  }
922 
923  if (!m_ftControlThreadIsRunning) {
924  getRobotInternalState(); // Update m_robot_state internally
925  m_ftControlThreadIsRunning = true;
926  m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
927  std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
928  std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
929  std::ref(m_mutex), filter_gain, activate_pi_controller);
930  }
931 }
932 
934 {
935  if (!m_handler) {
936  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
937  }
938  franka::RobotState robot_state;
939 
940  if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
941  robot_state = m_handler->readOnce();
942 
943  std::lock_guard<std::mutex> lock(m_mutex);
944  m_robot_state = robot_state;
945  } else { // robot_state is updated in the velocity control thread
946  std::lock_guard<std::mutex> lock(m_mutex);
947  robot_state = m_robot_state;
948  }
949 
950  return robot_state;
951 }
952 
959 {
960  vpColVector q_min(m_q_min.size());
961  for (size_t i = 0; i < m_q_min.size(); i++)
962  q_min[i] = m_q_min[i];
963 
964  return q_min;
965 }
972 {
973  vpColVector q_max(m_q_max.size());
974  for (size_t i = 0; i < m_q_max.size(); i++)
975  q_max[i] = m_q_max[i];
976 
977  return q_max;
978 }
979 
991 
1004 void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc) { m_eMc = eMc; }
1005 
1015 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1016 {
1017  vpColVector q;
1018 
1019  this->readPosFile(filename, q);
1021  this->setPositioningVelocity(velocity_percentage);
1023 }
1024 
1069 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1070 {
1071  std::ifstream fd(filename.c_str(), std::ios::in);
1072 
1073  if (!fd.is_open()) {
1074  return false;
1075  }
1076 
1077  std::string line;
1078  std::string key("R:");
1079  std::string id("#PANDA - Joint position file");
1080  bool pos_found = false;
1081  int lineNum = 0;
1082  size_t njoints = 7;
1083 
1084  q.resize(njoints);
1085 
1086  while (std::getline(fd, line)) {
1087  lineNum++;
1088  if (lineNum == 1) {
1089  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1090  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1091  return false;
1092  }
1093  }
1094  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1095  continue;
1096  }
1097  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1098  // check if there are at least njoint values in the line
1099  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1100  if (chain.size() < njoints + 1) // try to split with tab separator
1101  chain = vpIoTools::splitChain(line, std::string("\t"));
1102  if (chain.size() < njoints + 1)
1103  continue;
1104 
1105  std::istringstream ss(line);
1106  std::string key_;
1107  ss >> key_;
1108  for (unsigned int i = 0; i < njoints; i++)
1109  ss >> q[i];
1110  pos_found = true;
1111  break;
1112  }
1113  }
1114 
1115  // converts rotations from degrees into radians
1116  for (unsigned int i = 0; i < njoints; i++) {
1117  q[i] = vpMath::rad(q[i]);
1118  }
1119 
1120  fd.close();
1121 
1122  if (!pos_found) {
1123  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1124  return false;
1125  }
1126 
1127  return true;
1128 }
1129 
1152 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1153 {
1154 
1155  FILE *fd;
1156  fd = fopen(filename.c_str(), "w");
1157  if (fd == NULL)
1158  return false;
1159 
1160  fprintf(fd, "#PANDA - Joint position file\n"
1161  "#\n"
1162  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1163  "# with joint positions q1 to q7 expressed in degrees\n"
1164  "#\n");
1165 
1166  // Save positions in mm and deg
1167  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1168  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1169 
1170  fclose(fd);
1171  return (true);
1172 }
1173 
1182 {
1184  vpColVector q(7, 0);
1186  }
1188 }
1189 
1197 {
1198  if (m_franka_address.empty()) {
1199  throw(vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1200  }
1201  if (m_gripper == NULL)
1202  m_gripper = new franka::Gripper(m_franka_address);
1203 
1204  m_gripper->homing();
1205 }
1206 
1217 {
1218  if (m_franka_address.empty()) {
1219  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1220  }
1221  if (m_gripper == NULL)
1222  m_gripper = new franka::Gripper(m_franka_address);
1223 
1224  // Check for the maximum grasping width.
1225  franka::GripperState gripper_state = m_gripper->readOnce();
1226 
1227  if (gripper_state.max_width < width) {
1228  std::cout << "Finger width request is too large for the current fingers on the gripper."
1229  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1230  return EXIT_FAILURE;
1231  }
1232 
1233  m_gripper->move(width, 0.1);
1234  return EXIT_SUCCESS;
1235 }
1236 
1246 
1256 {
1257  if (m_franka_address.empty()) {
1258  throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1259  }
1260  if (m_gripper == NULL)
1261  m_gripper = new franka::Gripper(m_franka_address);
1262 
1263  // Check for the maximum grasping width.
1264  franka::GripperState gripper_state = m_gripper->readOnce();
1265 
1266  m_gripper->move(gripper_state.max_width, 0.1);
1267  return EXIT_SUCCESS;
1268 }
1269 
1277 {
1278  if (m_franka_address.empty()) {
1279  throw(vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1280  }
1281  if (m_gripper == NULL)
1282  m_gripper = new franka::Gripper(m_franka_address);
1283 
1284  m_gripper->stop();
1285 }
1286 
1301 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1302 {
1303  if (m_gripper == NULL)
1304  m_gripper = new franka::Gripper(m_franka_address);
1305 
1306  // Check for the maximum grasping width.
1307  franka::GripperState gripper_state = m_gripper->readOnce();
1308  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1309  if (gripper_state.max_width < grasping_width) {
1310  std::cout << "Object is too large for the current fingers on the gripper."
1311  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1312  return EXIT_FAILURE;
1313  }
1314 
1315  // Grasp the object.
1316  if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1317  std::cout << "Failed to grasp object." << std::endl;
1318  return EXIT_FAILURE;
1319  }
1320 
1321  return EXIT_SUCCESS;
1322 }
1323 
1324 #elif !defined(VISP_BUILD_SHARED_LIBS)
1325 // Work around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1326 // no symbols
1327 void dummy_vpRobotFranka(){};
1328 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ fatalError
Fatal error.
Definition: vpException.h:96
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:1926
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:431
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:581
static double rad(double deg)
Definition: vpMath.h:117
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Error that can be emited by the vpRobot class and its derivates.
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
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 get_eJe(vpMatrix &eJe)
void getCoriolis(vpColVector &coriolis)
void setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe)
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
virtual ~vpRobotFranka()
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
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:60
int nDof
number of degrees of freedom
Definition: vpRobot.h:103
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ JOINT_STATE
Definition: vpRobot.h:81
@ TOOL_FRAME
Definition: vpRobot.h:85
@ MIXT_FRAME
Definition: vpRobot.h:87
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
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:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251