Visual Servoing Platform version 3.5.0
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/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
74vpRobotFranka::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
90void 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
129void 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);
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);
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{
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 if (frame == JOINT_STATE) {
457 throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
458 }
459
460 franka::RobotState robot_state = getRobotInternalState();
461
462 std::array<double, 16> pose_array = robot_state.O_T_EE;
464 for (unsigned int i=0; i< 4; i++) {
465 for (unsigned int j=0; j< 4; j++) {
466 fMe[i][j] = pose_array[j*4 + i];
467 }
468 }
469
470 switch(frame) {
471 case END_EFFECTOR_FRAME: {
472 pose.buildFrom(fMe);
473 break;
474 }
475 case TOOL_FRAME: {
476 pose.buildFrom(fMe * m_eMc);
477 break;
478 }
479 default: {
480 throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
481 }
482 }
483}
484
491{
492 if (!m_handler) {
493 throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
494 }
495
496 franka::RobotState robot_state = getRobotInternalState();
497
498 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
499 eJe_.resize(6, 7); // row-major
500 for (size_t i = 0; i < 6; i ++) {
501 for (size_t j = 0; j < 7; j ++) {
502 eJe_[i][j] = jacobian[j*6 + i];
503 }
504 }
505 // TODO check from vpRobot fJe and fJeAvailable
506}
507
515{
516 if (!m_handler) {
517 throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
518 }
519
520 franka::RobotState robot_state = getRobotInternalState();
521
522 std::array< double, 7 > q_array;
523 for (size_t i = 0; i < 7; i++)
524 q_array[i] = q[i];
525
526 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
527 eJe_.resize(6, 7); // row-major
528 for (size_t i = 0; i < 6; i ++) {
529 for (size_t j = 0; j < 7; j ++) {
530 eJe_[i][j] = jacobian[j*6 + i];
531 }
532 }
533 // TODO check from vpRobot fJe and fJeAvailable
534
535}
536
543{
544 if (!m_handler) {
545 throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
546 }
547
548 franka::RobotState robot_state = getRobotInternalState();
549
550 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
551 fJe_.resize(6, 7); // row-major
552 for (size_t i = 0; i < 6; i ++) {
553 for (size_t j = 0; j < 7; j ++) {
554 fJe_[i][j] = jacobian[j*6 + i];
555 }
556 }
557 // TODO check from vpRobot fJe and fJeAvailable
558}
559
567{
568 if (!m_handler) {
569 throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
570 }
571 if (q.size() != 7) {
572 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()));
573 }
574
575 franka::RobotState robot_state = getRobotInternalState();
576
577 std::array< double, 7 > q_array;
578 for (size_t i = 0; i < 7; i++)
579 q_array[i] = q[i];
580
581 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
582 fJe_.resize(6, 7); // row-major
583 for (size_t i = 0; i < 6; i ++) {
584 for (size_t j = 0; j < 7; j ++) {
585 fJe_[i][j] = jacobian[j*6 + i];
586 }
587 }
588 // TODO check from vpRobot fJe and fJeAvailable
589}
590
600void vpRobotFranka::setLogFolder(const std::string &folder)
601{
602 if (!folder.empty()) {
603 if (vpIoTools::checkDirectory(folder) == false) {
604 try {
606 m_log_folder = folder;
607 }
608 catch(const vpException &e) {
609 std::string error;
610 error = "Unable to create Franka log folder: " + folder;
612 }
613 }
614 else {
615 m_log_folder = folder;
616 }
617 }
618}
619
626{
627 if (!m_handler) {
628 throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
629 }
631 std::cout << "Robot was not in position-based control. "
632 "Modification of the robot state" << std::endl;
634 }
635
636 if (frame == vpRobot::JOINT_STATE) {
637 double speed_factor = m_positionningVelocity / 100.;
638
639 std::array<double, 7> q_goal;
640 for (size_t i = 0; i < 7; i++) {
641 q_goal[i] = position[i];
642 }
643
644 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
645
646 int nbAttempts = 10;
647 for (int attempt = 1; attempt <= nbAttempts; attempt++) {
648 try {
649 m_handler->control(joint_pos_traj_generator);
650 break;
651 } catch (const franka::ControlException &e) {
652 std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
653 m_handler->automaticErrorRecovery();
654 if (attempt == nbAttempts)
655 throw;
656 }
657 }
658 }
659 else {
661 "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
662 }
663}
664
674{
675 m_positionningVelocity = velocity;
676}
677
685{
686 switch (newState) {
687 case vpRobot::STATE_STOP: {
688 // Start primitive STOP only if the current state is velocity or force/torque
690 // Stop the robot
691 m_velControlThreadStopAsked = true;
692 if(m_velControlThread.joinable()) {
693 m_velControlThread.join();
694 m_velControlThreadStopAsked = false;
695 m_velControlThreadIsRunning = false;
696 }
697 }
699 // Stop the robot
700 m_ftControlThreadStopAsked = true;
701 if(m_ftControlThread.joinable()) {
702 m_ftControlThread.join();
703 m_ftControlThreadStopAsked = false;
704 m_ftControlThreadIsRunning = false;
705 }
706 }
707 break;
708 }
711 std::cout << "Change the control mode from velocity to position control.\n";
712 // Stop the robot
713 m_velControlThreadStopAsked = true;
714 if(m_velControlThread.joinable()) {
715 m_velControlThread.join();
716 m_velControlThreadStopAsked = false;
717 m_velControlThreadIsRunning = false;
718 }
719 }
721 std::cout << "Change the control mode from force/torque to position 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 velocity control.\n";
735 }
737 std::cout << "Change the control mode from position to velocity control.\n";
738 }
740 std::cout << "Change the control mode from force/torque to velocity control.\n";
741 // Stop the robot
742 m_ftControlThreadStopAsked = true;
743 if(m_ftControlThread.joinable()) {
744 m_ftControlThread.join();
745 m_ftControlThreadStopAsked = false;
746 m_ftControlThreadIsRunning = false;
747 }
748 }
749 break;
750 }
753 std::cout << "Change the control mode from stop to force/torque control.\n";
754 }
756 std::cout << "Change the control mode from position to force/torque control.\n";
757 }
759 std::cout << "Change the control mode from velocity to force/torque control.\n";
760 // Stop the robot
761 m_velControlThreadStopAsked = true;
762 if(m_velControlThread.joinable()) {
763 m_velControlThread.join();
764 m_velControlThreadStopAsked = false;
765 m_velControlThreadIsRunning = false;
766 }
767 }
768 break;
769 }
770
771 default:
772 break;
773 }
774
775 return vpRobot::setRobotState(newState);
776}
777
832{
835 "Cannot send a velocity to the robot. "
836 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
837 }
838
839 switch (frame) {
840 // Saturation in joint space
841 case JOINT_STATE: {
842 if (vel.size() != 7) {
844 "Joint velocity vector (%d) is not of size 7", vel.size());
845 }
846
848
849 vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
850
851 for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
852 m_dq_des[i] = vel_sat[i];
853 }
854
855 break;
856 }
857
858 // Saturation in cartesian space
862 if (vel.size() != 6) {
864 "Cartesian velocity vector (%d) is not of size 6", vel.size());
865 }
866 vpColVector vel_max(6);
867
868 for (unsigned int i = 0; i < 3; i++)
869 vel_max[i] = getMaxTranslationVelocity();
870 for (unsigned int i = 3; i < 6; i++)
871 vel_max[i] = getMaxRotationVelocity();
872
873 m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
874
875 break;
876 }
877
878 case vpRobot::MIXT_FRAME: {
880 "Velocity controller not supported");
881 }
882 }
883
884 if(! m_velControlThreadIsRunning) {
885 m_velControlThreadIsRunning = true;
886 m_velControlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
887 std::ref(m_handler), std::ref(m_velControlThreadStopAsked), m_log_folder,
888 frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
889 std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
890 std::ref(m_robot_state), std::ref(m_mutex));
891 }
892}
893
894/*
895 Apply a force/torque to the robot.
896
897 \param[in] frame : Control frame in which the force/torque is applied.
898
899 \param[in] ft : Force/torque vector. The size of this vector
900 is always 6 for a cartesian force/torque skew, and 7 for joint torques.
901
902 \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
903 To diable the filter set filter_gain to 1.
904 \param[in] activate_pi_controller : Activate proportional and integral low level controller.
905 */
907 const double &filter_gain, const bool &activate_pi_controller)
908{
909 switch (frame) {
910 // Saturation in joint space
911 case JOINT_STATE: {
912 if (ft.size() != 7) {
914 "Joint torques vector (%d) is not of size 7", ft.size());
915 }
916
917 for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
918 m_tau_J_des[i] = ft[i];
919 }
920 // TODO: Introduce force/torque saturation
921
922 break;
923 }
924
925 // Saturation in cartesian space
929 if (ft.size() != 6) {
931 "Cartesian force/torque vector (%d) is not of size 6", ft.size());
932 }
933
934 m_ft_cart_des = ft;
935 // TODO: Introduce force/torque saturation
936
937 break;
938 }
939
940 case vpRobot::MIXT_FRAME: {
942 "Velocity controller not supported");
943 }
944 }
945
946 if(! m_ftControlThreadIsRunning) {
947 getRobotInternalState(); // Update m_robot_state internally
948 m_ftControlThreadIsRunning = true;
949 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
950 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder,
951 frame, std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
952 std::ref(m_mutex), filter_gain, activate_pi_controller);
953 }
954}
955
957{
958 if (!m_handler) {
959 throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
960 }
961 franka::RobotState robot_state;
962
963 if (! m_velControlThreadIsRunning && ! m_ftControlThreadIsRunning) {
964 robot_state = m_handler->readOnce();
965
966 std::lock_guard<std::mutex> lock(m_mutex);
967 m_robot_state = robot_state;
968 }
969 else { // robot_state is updated in the velocity control thread
970 std::lock_guard<std::mutex> lock(m_mutex);
971 robot_state = m_robot_state;
972 }
973
974 return robot_state;
975}
976
983{
984 vpColVector q_min(m_q_min.size());
985 for (size_t i = 0; i < m_q_min.size(); i ++)
986 q_min[i] = m_q_min[i];
987
988 return q_min;
989}
996{
997 vpColVector q_max(m_q_max.size());
998 for (size_t i = 0; i < m_q_max.size(); i ++)
999 q_max[i] = m_q_max[i];
1000
1001 return q_max;
1002}
1003
1015{
1016 return m_eMc;
1017}
1018
1032{
1033 m_eMc = eMc;
1034}
1035
1045void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1046{
1047 vpColVector q;
1048
1049 this->readPosFile(filename, q);
1051 this->setPositioningVelocity(velocity_percentage);
1053}
1054
1099bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1100{
1101 std::ifstream fd(filename.c_str(), std::ios::in);
1102
1103 if (!fd.is_open()) {
1104 return false;
1105 }
1106
1107 std::string line;
1108 std::string key("R:");
1109 std::string id("#PANDA - Joint position file");
1110 bool pos_found = false;
1111 int lineNum = 0;
1112 size_t njoints = 7;
1113
1114 q.resize(njoints);
1115
1116 while (std::getline(fd, line)) {
1117 lineNum++;
1118 if (lineNum == 1) {
1119 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1120 std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1121 return false;
1122 }
1123 }
1124 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1125 continue;
1126 }
1127 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1128 // check if there are at least njoint values in the line
1129 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1130 if (chain.size() < njoints + 1) // try to split with tab separator
1131 chain = vpIoTools::splitChain(line, std::string("\t"));
1132 if (chain.size() < njoints + 1)
1133 continue;
1134
1135 std::istringstream ss(line);
1136 std::string key_;
1137 ss >> key_;
1138 for (unsigned int i = 0; i < njoints; i++)
1139 ss >> q[i];
1140 pos_found = true;
1141 break;
1142 }
1143 }
1144
1145 // converts rotations from degrees into radians
1146 for (unsigned int i = 0; i < njoints; i++) {
1147 q[i] = vpMath::rad(q[i]);
1148 }
1149
1150 fd.close();
1151
1152 if (!pos_found) {
1153 std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1154 return false;
1155 }
1156
1157 return true;
1158}
1159
1182bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1183{
1184
1185 FILE *fd;
1186 fd = fopen(filename.c_str(), "w");
1187 if (fd == NULL)
1188 return false;
1189
1190 fprintf(fd,
1191 "#PANDA - Joint position file\n"
1192 "#\n"
1193 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1194 "# with joint positions q1 to q7 expressed in degrees\n"
1195 "#\n");
1196
1197 // Save positions in mm and deg
1198 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1199 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1200
1201 fclose(fd);
1202 return (true);
1203}
1204
1213{
1215 vpColVector q(7, 0);
1217 }
1219}
1220
1228{
1229 if (m_franka_address.empty()) {
1230 throw (vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1231 }
1232 if (m_gripper == NULL)
1233 m_gripper = new franka::Gripper(m_franka_address);
1234
1235 m_gripper->homing();
1236}
1237
1248{
1249 if (m_franka_address.empty()) {
1250 throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1251 }
1252 if (m_gripper == NULL)
1253 m_gripper = new franka::Gripper(m_franka_address);
1254
1255 // Check for the maximum grasping width.
1256 franka::GripperState gripper_state = m_gripper->readOnce();
1257
1258 if (gripper_state.max_width < width) {
1259 std::cout << "Finger width request is too large for the current fingers on the gripper."
1260 << "Maximum possible width is " << gripper_state.max_width << std::endl;
1261 return EXIT_FAILURE;
1262 }
1263
1264 m_gripper->move(width, 0.1);
1265 return EXIT_SUCCESS;
1266}
1267
1277{
1278 return gripperMove(0);
1279}
1280
1290{
1291 if (m_franka_address.empty()) {
1292 throw (vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1293 }
1294 if (m_gripper == NULL)
1295 m_gripper = new franka::Gripper(m_franka_address);
1296
1297 // Check for the maximum grasping width.
1298 franka::GripperState gripper_state = m_gripper->readOnce();
1299
1300 m_gripper->move(gripper_state.max_width, 0.1);
1301 return EXIT_SUCCESS;
1302}
1303
1311{
1312 if (m_franka_address.empty()) {
1313 throw (vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1314 }
1315 if (m_gripper == NULL)
1316 m_gripper = new franka::Gripper(m_franka_address);
1317
1318 m_gripper->stop();
1319}
1320
1335int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1336{
1337 if (m_gripper == NULL)
1338 m_gripper = new franka::Gripper(m_franka_address);
1339
1340 // Check for the maximum grasping width.
1341 franka::GripperState gripper_state = m_gripper->readOnce();
1342 std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1343 if (gripper_state.max_width < grasping_width) {
1344 std::cout << "Object is too large for the current fingers on the gripper."
1345 << "Maximum possible width is " << gripper_state.max_width << std::endl;
1346 return EXIT_FAILURE;
1347 }
1348
1349 // Grasp the object.
1350 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1351 std::cout << "Failed to grasp object." << std::endl;
1352 return EXIT_FAILURE;
1353 }
1354
1355 return EXIT_SUCCESS;
1356}
1357
1358#elif !defined(VISP_BUILD_SHARED_LIBS)
1359// Work arround to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1360// no symbols
1361void dummy_vpRobotFranka(){};
1362#endif
1363
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:1900
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
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:59
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
@ STATE_FORCE_TORQUE_CONTROL
Initialize the force/torque controller.
Definition: vpRobot.h:69
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