Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpRobotFranka.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Franka robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_FRANKA
42 
43 #include <visp3/robot/vpRobotException.h>
44 #include <visp3/robot/vpRobotFranka.h>
45 #include <visp3/core/vpIoTools.h>
46 
47 #include "vpJointPosTrajGenerator_impl.h"
48 #include "vpJointVelTrajGenerator_impl.h"
49 #include "vpForceTorqueGenerator_impl.h"
50 
57  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
58  m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
59  m_dq_des(), m_v_cart_des(),
60  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
61  m_tau_J_des(), m_ft_cart_des(),
62  m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
63  m_mutex(), m_eMc(), m_log_folder(), m_franka_address()
64 {
65  init();
66 }
67 
74 vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
75  : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
76  m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
77  m_dq_des(), m_v_cart_des(),
78  m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
79  m_tau_J_des(), m_ft_cart_des(),
80  m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
81  m_mutex(), m_eMc(),m_log_folder(), m_franka_address()
82 {
83  init();
84  connect(franka_address, realtime_config);
85 }
86 
90 void vpRobotFranka::init()
91 {
92  nDof = 7;
93 
94  m_q_min = std::array<double, 7> {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
95  m_q_max = std::array<double, 7> {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
96  m_dq_max = std::array<double, 7> {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
97  m_ddq_max = std::array<double, 7> {15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
98 }
99 
106 {
108 
109  if (m_handler)
110  delete m_handler;
111 
112  if (m_gripper) {
113  std::cout << "Grasped object, will release it now." << std::endl;
114  m_gripper->stop();
115  delete m_gripper;
116  }
117 
118  if (m_model) {
119  delete m_model;
120  }
121 }
122 
129 void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
130 {
131  init();
132  if(franka_address.empty()) {
133  throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
134  }
135  if (m_handler)
136  delete m_handler;
137 
138  m_franka_address = franka_address;
139  m_handler = new franka::Robot(franka_address, realtime_config);
140 
141  std::array<double, 7> lower_torque_thresholds_nominal{
142  {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
143  std::array<double, 7> upper_torque_thresholds_nominal{
144  {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
145  std::array<double, 7> lower_torque_thresholds_acceleration{
146  {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
147  std::array<double, 7> upper_torque_thresholds_acceleration{
148  {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
149  std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
150  std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
151  std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
152  std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
153  m_handler->setCollisionBehavior(
154  lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
155  lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
156  lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
157  lower_force_thresholds_nominal, upper_force_thresholds_nominal);
158 
159  m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
160  m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
161 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
162  // m_handler->setFilters(100, 100, 100, 100, 100);
163  m_handler->setFilters(10, 10, 10, 10, 10);
164 #else
165  // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
166 #endif
167  if (m_model) {
168  delete m_model;
169  }
170  m_model = new franka::Model(m_handler->loadModel());
171 }
172 
188 {
189  if (!m_handler) {
190  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
191  }
192 
193  franka::RobotState robot_state = getRobotInternalState();
194  vpColVector q(7);
195  for (int i=0; i < 7; i++) {
196  q[i] = robot_state.q[i];
197  }
198 
199  switch(frame) {
200  case JOINT_STATE: {
201  position = q;
202  break;
203  }
204  case END_EFFECTOR_FRAME: {
205  position.resize(6);
206  vpHomogeneousMatrix fMe = get_fMe(q);
207  vpPoseVector fPe(fMe);
208  for (size_t i=0; i < 6; i++) {
209  position[i] = fPe[i];
210  }
211  break;
212  }
213  case TOOL_FRAME: { // same a CAMERA_FRAME
214  position.resize(6);
215  vpHomogeneousMatrix fMc = get_fMc(q);
216  vpPoseVector fPc(fMc);
217  for (size_t i=0; i < 6; i++) {
218  position[i] = fPc[i];
219  }
220  break;
221  }
222  default: {
223  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
224  }
225  }
226 }
227 
242 {
243  if (!m_handler) {
244  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
245  }
246 
247  franka::RobotState robot_state = getRobotInternalState();
248 
249  switch(frame) {
250  case JOINT_STATE: {
251  force.resize(7);
252  for (int i=0; i < 7; i++)
253  force[i] = robot_state.tau_J[i];
254 
255  break;
256  }
257  case END_EFFECTOR_FRAME: {
258  force.resize(6);
259  for (int i=0; i < 6; i++)
260  force[i] = robot_state.K_F_ext_hat_K[i];
261  break;
262  }
263  case TOOL_FRAME: {
264  // end-effector frame
265  vpColVector eFe(6);
266  for (int i=0; i < 6; i++)
267  eFe[i] = robot_state.K_F_ext_hat_K[i];
268 
269  // Transform in tool frame
271  vpForceTwistMatrix cWe( cMe );
272  force = cWe * eFe;
273  break;
274  }
275  default: {
276  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
277  }
278  }
279 }
280 
296 {
297  if (!m_handler) {
298  throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
299  }
300 
301  franka::RobotState robot_state = getRobotInternalState();
302 
303  switch(frame) {
304 
305  case JOINT_STATE: {
306  d_position.resize(7);
307  for (int i=0; i < 7; i++) {
308  d_position[i]=robot_state.dq[i];
309  }
310  break;
311  }
312 
313  default: {
314  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
315  }
316  }
317 }
318 
325 {
326  if (!m_handler) {
327  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
328  }
329 
330  std::array<double, 7> coriolis_;
331 
332  franka::RobotState robot_state = getRobotInternalState();
333 
334  coriolis_ = m_model->coriolis(robot_state);
335 
336  coriolis.resize(7);
337  for (int i=0; i < 7; i++) {
338  coriolis[i] = coriolis_[i];
339  }
340 }
341 
347 {
348  if (!m_handler) {
349  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
350  }
351 
352  std::array<double, 7> gravity_;
353 
354  franka::RobotState robot_state = getRobotInternalState();
355 
356  gravity_ = m_model->gravity(robot_state);
357 
358  gravity.resize(7);
359  for (int i=0; i < 7; i++) {
360  gravity[i] = gravity_[i];
361  }
362 }
363 
369 {
370  if (!m_handler) {
371  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
372  }
373 
374  std::array<double, 49> mass_;
375 
376  franka::RobotState robot_state = getRobotInternalState();
377 
378  mass_ = m_model->mass(robot_state); // column-major
379 
380  mass.resize(7, 7); // row-major
381  for (size_t i = 0; i < 7; i ++) {
382  for (size_t j = 0; j < 7; j ++) {
383  mass[i][j] = mass_[j*7 + i];
384  }
385  }
386 }
387 
396 {
397  if (!m_handler) {
398  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
399  }
400  if (q.size() != 7) {
401  throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
402  }
403 
404  std::array< double, 7 > q_array;
405  for (size_t i = 0; i < 7; i++)
406  q_array[i] = q[i];
407 
408  franka::RobotState robot_state = getRobotInternalState();
409 
410  std::array<double, 16> pose_array = m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
412  for (unsigned int i=0; i< 4; i++) {
413  for (unsigned int j=0; j< 4; j++) {
414  fMe[i][j] = pose_array[j*4 + i];
415  }
416  }
417 
418  return fMe;
419 }
420 
436 {
437  vpHomogeneousMatrix fMe = get_fMe(q);
438  return (fMe * m_eMc);
439 }
440 
452 {
453  if (!m_handler) {
454  throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
455  }
456 
457  franka::RobotState robot_state = getRobotInternalState();
458 
459  std::array<double, 16> pose_array = robot_state.O_T_EE;
461  for (unsigned int i=0; i< 4; i++) {
462  for (unsigned int j=0; j< 4; j++) {
463  fMe[i][j] = pose_array[j*4 + i];
464  }
465  }
466 
467  switch(frame) {
468  case END_EFFECTOR_FRAME: {
469  pose.buildFrom(fMe);
470  break;
471  }
472  case TOOL_FRAME: {
473  pose.buildFrom(fMe * m_eMc);
474  break;
475  }
476  default: {
477  throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
478  }
479  }
480 }
481 
488 {
489  if (!m_handler) {
490  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
491  }
492 
493  franka::RobotState robot_state = getRobotInternalState();
494 
495  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
496  eJe.resize(6, 7); // row-major
497  for (size_t i = 0; i < 6; i ++) {
498  for (size_t j = 0; j < 7; j ++) {
499  eJe[i][j] = jacobian[j*6 + i];
500  }
501  }
502  // TODO check from vpRobot fJe and fJeAvailable
503 
504 }
505 
513 {
514  if (!m_handler) {
515  throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
516  }
517 
518  franka::RobotState robot_state = getRobotInternalState();
519 
520  std::array< double, 7 > q_array;
521  for (size_t i = 0; i < 7; i++)
522  q_array[i] = q[i];
523 
524  std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
525  eJe.resize(6, 7); // row-major
526  for (size_t i = 0; i < 6; i ++) {
527  for (size_t j = 0; j < 7; j ++) {
528  eJe[i][j] = jacobian[j*6 + i];
529  }
530  }
531  // TODO check from vpRobot fJe and fJeAvailable
532 
533 }
534 
541 {
542  if (!m_handler) {
543  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
544  }
545 
546  franka::RobotState robot_state = getRobotInternalState();
547 
548  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
549  fJe.resize(6, 7); // row-major
550  for (size_t i = 0; i < 6; i ++) {
551  for (size_t j = 0; j < 7; j ++) {
552  fJe[i][j] = jacobian[j*6 + i];
553  }
554  }
555  // TODO check from vpRobot fJe and fJeAvailable
556 }
557 
565 {
566  if (!m_handler) {
567  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
568  }
569  if (q.size() != 7) {
570  throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector", q.size()));
571  }
572 
573  franka::RobotState robot_state = getRobotInternalState();
574 
575  std::array< double, 7 > q_array;
576  for (size_t i = 0; i < 7; i++)
577  q_array[i] = q[i];
578 
579  std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K); // column-major
580  fJe.resize(6, 7); // row-major
581  for (size_t i = 0; i < 6; i ++) {
582  for (size_t j = 0; j < 7; j ++) {
583  fJe[i][j] = jacobian[j*6 + i];
584  }
585  }
586  // TODO check from vpRobot fJe and fJeAvailable
587 }
588 
598 void vpRobotFranka::setLogFolder(const std::string &folder)
599 {
600  if (!folder.empty()) {
601  if (vpIoTools::checkDirectory(folder) == false) {
602  try {
603  vpIoTools::makeDirectory(folder);
604  m_log_folder = folder;
605  }
606  catch(const vpException &e) {
607  std::string error;
608  error = "Unable to create Franka log folder: " + folder;
609  throw(vpException(vpException::fatalError, error));
610  }
611  }
612  else {
613  m_log_folder = folder;
614  }
615  }
616 }
617 
624 {
625  if (!m_handler) {
626  throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
627  }
629  std::cout << "Robot was not in position-based control. "
630  "Modification of the robot state" << std::endl;
632  }
633 
634  if (frame == vpRobot::JOINT_STATE) {
635  double speed_factor = m_positionningVelocity / 100.;
636 
637  std::array<double, 7> q_goal;
638  for (size_t i = 0; i < 7; i++) {
639  q_goal[i] = position[i];
640  }
641 
642  vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
643 
644  int nbAttempts = 10;
645  for (int attempt = 1; attempt <= nbAttempts; attempt++) {
646  try {
647  m_handler->control(joint_pos_traj_generator);
648  break;
649  } catch (const franka::ControlException &e) {
650  std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
651  m_handler->automaticErrorRecovery();
652  if (attempt == nbAttempts)
653  throw;
654  }
655  }
656  }
657  else {
659  "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
660  }
661 }
662 
671 void vpRobotFranka::setPositioningVelocity(const double velocity)
672 {
673  m_positionningVelocity = velocity;
674 }
675 
683 {
684  switch (newState) {
685  case vpRobot::STATE_STOP: {
686  // Start primitive STOP only if the current state is velocity or force/torque
688  // Stop the robot
689  m_velControlThreadStopAsked = true;
690  if(m_velControlThread.joinable()) {
691  m_velControlThread.join();
692  m_velControlThreadStopAsked = false;
693  m_velControlThreadIsRunning = false;
694  }
695  }
697  // Stop the robot
698  m_ftControlThreadStopAsked = true;
699  if(m_ftControlThread.joinable()) {
700  m_ftControlThread.join();
701  m_ftControlThreadStopAsked = false;
702  m_ftControlThreadIsRunning = false;
703  }
704  }
705  break;
706  }
709  std::cout << "Change the control mode from velocity to position control.\n";
710  // Stop the robot
711  m_velControlThreadStopAsked = true;
712  if(m_velControlThread.joinable()) {
713  m_velControlThread.join();
714  m_velControlThreadStopAsked = false;
715  m_velControlThreadIsRunning = false;
716  }
717  }
719  std::cout << "Change the control mode from force/torque to position control.\n";
720  // Stop the robot
721  m_ftControlThreadStopAsked = true;
722  if(m_ftControlThread.joinable()) {
723  m_ftControlThread.join();
724  m_ftControlThreadStopAsked = false;
725  m_ftControlThreadIsRunning = false;
726  }
727  }
728  break;
729  }
732  std::cout << "Change the control mode from stop to velocity control.\n";
733  }
735  std::cout << "Change the control mode from position to velocity control.\n";
736  }
738  std::cout << "Change the control mode from force/torque to velocity control.\n";
739  // Stop the robot
740  m_ftControlThreadStopAsked = true;
741  if(m_ftControlThread.joinable()) {
742  m_ftControlThread.join();
743  m_ftControlThreadStopAsked = false;
744  m_ftControlThreadIsRunning = false;
745  }
746  }
747  break;
748  }
751  std::cout << "Change the control mode from stop to force/torque control.\n";
752  }
754  std::cout << "Change the control mode from position to force/torque control.\n";
755  }
757  std::cout << "Change the control mode from velocity to force/torque control.\n";
758  // Stop the robot
759  m_velControlThreadStopAsked = true;
760  if(m_velControlThread.joinable()) {
761  m_velControlThread.join();
762  m_velControlThreadStopAsked = false;
763  m_velControlThreadIsRunning = false;
764  }
765  }
766  break;
767  }
768 
769  default:
770  break;
771  }
772 
773  return vpRobot::setRobotState(newState);
774 }
775 
830 {
833  "Cannot send a velocity to the robot. "
834  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
835  }
836 
837  switch (frame) {
838  // Saturation in joint space
839  case JOINT_STATE: {
840  if (vel.size() != 7) {
842  "Joint velocity vector (%d) is not of size 7", vel.size());
843  }
844 
845  vpColVector vel_max(7, getMaxRotationVelocity());
846 
847  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
848 
849  for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
850  m_dq_des[i] = vel_sat[i];
851  }
852 
853  break;
854  }
855 
856  // Saturation in cartesian space
857  case vpRobot::TOOL_FRAME:
860  if (vel.size() != 6) {
862  "Cartesian velocity vector (%d) is not of size 6", vel.size());
863  }
864  vpColVector vel_max(6);
865 
866  for (unsigned int i = 0; i < 3; i++)
867  vel_max[i] = getMaxTranslationVelocity();
868  for (unsigned int i = 3; i < 6; i++)
869  vel_max[i] = getMaxRotationVelocity();
870 
871  m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
872 
873  break;
874  }
875 
876  case vpRobot::MIXT_FRAME: {
878  "Velocity controller not supported");
879  }
880  }
881 
882  if(! m_velControlThreadIsRunning) {
883  m_velControlThreadIsRunning = true;
884  m_velControlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
885  std::ref(m_handler), std::ref(m_velControlThreadStopAsked), m_log_folder,
886  frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
887  std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
888  std::ref(m_robot_state), std::ref(m_mutex));
889  }
890 }
891 
892 /*
893  Apply a force/torque to the robot.
894 
895  \param[in] frame : Control frame in which the force/torque is applied.
896 
897  \param[in] ft : Force/torque vector. The size of this vector
898  is always 6 for a cartesian force/torque skew, and 7 for joint torques.
899 
900  \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
901  To diable the filter set filter_gain to 1.
902  \param[in] activate_pi_controller : Activate proportional and integral low level controller.
903  */
905  const double &filter_gain, const bool &activate_pi_controller)
906 {
907  switch (frame) {
908  // Saturation in joint space
909  case JOINT_STATE: {
910  if (ft.size() != 7) {
912  "Joint torques vector (%d) is not of size 7", ft.size());
913  }
914 
915  for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
916  m_tau_J_des[i] = ft[i];
917  }
918  // TODO: Introduce force/torque saturation
919 
920  break;
921  }
922 
923  // Saturation in cartesian space
924  case vpRobot::TOOL_FRAME:
927  if (ft.size() != 6) {
929  "Cartesian force/torque vector (%d) is not of size 6", ft.size());
930  }
931 
932  m_ft_cart_des = ft;
933  // TODO: Introduce force/torque saturation
934 
935  break;
936  }
937 
938  case vpRobot::MIXT_FRAME: {
940  "Velocity controller not supported");
941  }
942  }
943 
944  if(! m_ftControlThreadIsRunning) {
945  getRobotInternalState(); // Update m_robot_state internally
946  m_ftControlThreadIsRunning = true;
947  m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
948  std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder,
949  frame, std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
950  std::ref(m_mutex), filter_gain, activate_pi_controller);
951  }
952 }
953 
955 {
956  if (!m_handler) {
957  throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
958  }
959  franka::RobotState robot_state;
960 
961  if (! m_velControlThreadIsRunning && ! m_ftControlThreadIsRunning) {
962  robot_state = m_handler->readOnce();
963 
964  std::lock_guard<std::mutex> lock(m_mutex);
965  m_robot_state = robot_state;
966  }
967  else { // robot_state is updated in the velocity control thread
968  std::lock_guard<std::mutex> lock(m_mutex);
969  robot_state = m_robot_state;
970  }
971 
972  return robot_state;
973 }
974 
981 {
982  vpColVector q_min(m_q_min.size());
983  for (size_t i = 0; i < m_q_min.size(); i ++)
984  q_min[i] = m_q_min[i];
985 
986  return q_min;
987 }
994 {
995  vpColVector q_max(m_q_max.size());
996  for (size_t i = 0; i < m_q_max.size(); i ++)
997  q_max[i] = m_q_max[i];
998 
999  return q_max;
1000 }
1001 
1013 {
1014  return m_eMc;
1015 }
1016 
1030 {
1031  m_eMc = eMc;
1032 }
1033 
1043 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1044 {
1045  vpColVector q;
1046 
1047  this->readPosFile(filename, q);
1049  this->setPositioningVelocity(velocity_percentage);
1051 }
1052 
1097 bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1098 {
1099  std::ifstream fd(filename.c_str(), std::ios::in);
1100 
1101  if (!fd.is_open()) {
1102  return false;
1103  }
1104 
1105  std::string line;
1106  std::string key("R:");
1107  std::string id("#PANDA - Joint position file");
1108  bool pos_found = false;
1109  int lineNum = 0;
1110  size_t njoints = 7;
1111 
1112  q.resize(njoints);
1113 
1114  while (std::getline(fd, line)) {
1115  lineNum++;
1116  if (lineNum == 1) {
1117  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1118  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1119  return false;
1120  }
1121  }
1122  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1123  continue;
1124  }
1125  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1126  // check if there are at least njoint values in the line
1127  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1128  if (chain.size() < njoints + 1) // try to split with tab separator
1129  chain = vpIoTools::splitChain(line, std::string("\t"));
1130  if (chain.size() < njoints + 1)
1131  continue;
1132 
1133  std::istringstream ss(line);
1134  std::string key_;
1135  ss >> key_;
1136  for (unsigned int i = 0; i < njoints; i++)
1137  ss >> q[i];
1138  pos_found = true;
1139  break;
1140  }
1141  }
1142 
1143  // converts rotations from degrees into radians
1144  for (unsigned int i = 0; i < njoints; i++) {
1145  q[i] = vpMath::rad(q[i]);
1146  }
1147 
1148  fd.close();
1149 
1150  if (!pos_found) {
1151  std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1152  return false;
1153  }
1154 
1155  return true;
1156 }
1157 
1180 bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1181 {
1182 
1183  FILE *fd;
1184  fd = fopen(filename.c_str(), "w");
1185  if (fd == NULL)
1186  return false;
1187 
1188  fprintf(fd,
1189  "#PANDA - Joint position file\n"
1190  "#\n"
1191  "# R: q1 q2 q3 q4 q5 q6 q7\n"
1192  "# with joint positions q1 to q7 expressed in degrees\n"
1193  "#\n");
1194 
1195  // Save positions in mm and deg
1196  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1197  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1198 
1199  fclose(fd);
1200  return (true);
1201 }
1202 
1211 {
1213  vpColVector q(7, 0);
1215  }
1217 }
1218 
1226 {
1227  if (m_franka_address.empty()) {
1228  throw (vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1229  }
1230  if (m_gripper == NULL)
1231  m_gripper = new franka::Gripper(m_franka_address);
1232 
1233  m_gripper->homing();
1234 }
1235 
1246 {
1247  if (m_franka_address.empty()) {
1248  throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1249  }
1250  if (m_gripper == NULL)
1251  m_gripper = new franka::Gripper(m_franka_address);
1252 
1253  // Check for the maximum grasping width.
1254  franka::GripperState gripper_state = m_gripper->readOnce();
1255 
1256  if (gripper_state.max_width < width) {
1257  std::cout << "Finger width request is too large for the current fingers on the gripper."
1258  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1259  return EXIT_FAILURE;
1260  }
1261 
1262  m_gripper->move(width, 0.1);
1263  return EXIT_SUCCESS;
1264 }
1265 
1275 {
1276  return gripperMove(0);
1277 }
1278 
1288 {
1289  if (m_franka_address.empty()) {
1290  throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1291  }
1292  if (m_gripper == NULL)
1293  m_gripper = new franka::Gripper(m_franka_address);
1294 
1295  // Check for the maximum grasping width.
1296  franka::GripperState gripper_state = m_gripper->readOnce();
1297 
1298  m_gripper->move(gripper_state.max_width, 0.1);
1299  return EXIT_SUCCESS;
1300 }
1301 
1309 {
1310  if (m_franka_address.empty()) {
1311  throw (vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1312  }
1313  if (m_gripper == NULL)
1314  m_gripper = new franka::Gripper(m_franka_address);
1315 
1316  m_gripper->stop();
1317 }
1318 
1333 int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1334 {
1335  if (m_gripper == NULL)
1336  m_gripper = new franka::Gripper(m_franka_address);
1337 
1338  // Check for the maximum grasping width.
1339  franka::GripperState gripper_state = m_gripper->readOnce();
1340  std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1341  if (gripper_state.max_width < grasping_width) {
1342  std::cout << "Object is too large for the current fingers on the gripper."
1343  << "Maximum possible width is " << gripper_state.max_width << std::endl;
1344  return EXIT_FAILURE;
1345  }
1346 
1347  // Grasp the object.
1348  if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1349  std::cout << "Failed to grasp object." << std::endl;
1350  return EXIT_FAILURE;
1351  }
1352 
1353  return EXIT_SUCCESS;
1354 }
1355 
1356 #elif !defined(VISP_BUILD_SHARED_LIBS)
1357 // Work arround to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1358 // no symbols
1359 void dummy_vpRobotFranka(){};
1360 #endif
1361 
int gripperMove(double width)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:591
vpColVector getJointMax() const
Error that can be emited by the vpRobot class and its derivates.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void getCoriolis(vpColVector &coriolis)
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool savePosFile(const std::string &filename, const vpColVector &q)
virtual ~vpRobotFranka()
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
vpHomogeneousMatrix get_fMe(const vpColVector &q)
void getGravity(vpColVector &gravity)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
vpHomogeneousMatrix get_eMc() const
Initialize the position controller.
Definition: vpRobot.h:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
franka::RobotState getRobotInternalState()
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
int gripperGrasp(double grasping_width, double force=60.)
void getMass(vpMatrix &mass)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:441
void set_eMc(const vpHomogeneousMatrix &eMc)
Initialize the velocity controller.
Definition: vpRobot.h:66
vpRobotStateType
Definition: vpRobot.h:64
bool readPosFile(const std::string &filename, vpColVector &q)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1786
void get_eJe(vpMatrix &eJe)
static double rad(double deg)
Definition: vpMath.h:108
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void get_fJe(vpMatrix &fJe)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static double deg(double rad)
Definition: vpMath.h:101
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
void move(const std::string &filename, double velocity_percentage=10.)
Initialize the force/torque controller.
Definition: vpRobot.h:69
void setPositioningVelocity(const double velocity)
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft, const double &filter_gain=0.1, const bool &activate_pi_controller=false)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Function not implemented.
Definition: vpException.h:90
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310