Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotFranka.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Franka robot.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_FRANKA
39
40#include <visp3/core/vpIoTools.h>
41#include <visp3/robot/vpRobotException.h>
42#include <visp3/robot/vpRobotFranka.h>
43
44#include "vpForceTorqueGenerator_impl.h"
45#include "vpJointPosTrajGenerator_impl.h"
46#include "vpJointVelTrajGenerator_impl.h"
47
54 : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positioningVelocity(20.), m_velControlThread(),
55 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
56 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
57 m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
58 m_franka_address()
59{
60 init();
61}
62
69vpRobotFranka::vpRobotFranka(const std::string &franka_address, franka::RealtimeConfig realtime_config)
70 : vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positioningVelocity(20.), m_velControlThread(),
71 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
72 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
73 m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
74 m_franka_address()
75{
76 init();
77 connect(franka_address, realtime_config);
78}
79
83void vpRobotFranka::init()
84{
85 nDof = 7;
86
87 m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
88 m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
89 m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
90 m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
91}
92
99{
101
102 if (m_handler)
103 delete m_handler;
104
105 if (m_gripper) {
106 std::cout << "Grasped object, will release it now." << std::endl;
107 m_gripper->stop();
108 delete m_gripper;
109 }
110
111 if (m_model) {
112 delete m_model;
113 }
114}
115
122void vpRobotFranka::connect(const std::string &franka_address, franka::RealtimeConfig realtime_config)
123{
124 init();
125 if (franka_address.empty()) {
126 throw(vpException(vpException::fatalError, "Cannot connect Franka robot: IP/hostname is not set"));
127 }
128 if (m_handler)
129 delete m_handler;
130
131 m_franka_address = franka_address;
132 m_handler = new franka::Robot(franka_address, realtime_config);
133
134 std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
135 std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
136 std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
137 std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
138 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
139 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
140 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
141 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
142 m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
143 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
144 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
145 lower_force_thresholds_nominal, upper_force_thresholds_nominal);
146
147 m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
148 m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
149#if (VISP_HAVE_FRANKA_VERSION < 0x000500)
150 // m_handler->setFilters(100, 100, 100, 100, 100);
151 m_handler->setFilters(10, 10, 10, 10, 10);
152#else
153 // use franka::lowpassFilter() instead throw Franka::robot::control() with cutoff_frequency parameter
154#endif
155 if (m_model) {
156 delete m_model;
157 }
158 m_model = new franka::Model(m_handler->loadModel());
159}
160
176{
177 if (!m_handler) {
178 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
179 }
180
181 franka::RobotState robot_state = getRobotInternalState();
182 vpColVector q(7);
183 for (int i = 0; i < 7; i++) {
184 q[i] = robot_state.q[i];
185 }
186
187 switch (frame) {
188 case JOINT_STATE: {
189 position = q;
190 break;
191 }
192 case END_EFFECTOR_FRAME: {
193 position.resize(6);
195 vpPoseVector fPe(fMe);
196 for (size_t i = 0; i < 6; i++) {
197 position[i] = fPe[i];
198 }
199 break;
200 }
201 case TOOL_FRAME: { // same a CAMERA_FRAME
202 position.resize(6);
204 vpPoseVector fPc(fMc);
205 for (size_t i = 0; i < 6; i++) {
206 position[i] = fPc[i];
207 }
208 break;
209 }
210 default: {
211 throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: wrong method"));
212 }
213 }
214}
215
227{
228 if (!m_handler) {
229 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
230 }
231
232 franka::RobotState robot_state = getRobotInternalState();
233
234 switch (frame) {
235 case JOINT_STATE: {
236 force.resize(7);
237 for (int i = 0; i < 7; i++)
238 force[i] = robot_state.tau_J[i];
239
240 break;
241 }
242 case END_EFFECTOR_FRAME: {
243 force.resize(6);
244 for (int i = 0; i < 6; i++)
245 force[i] = robot_state.K_F_ext_hat_K[i];
246 break;
247 }
248 case TOOL_FRAME: {
249 // end-effector frame
250 vpColVector eFe(6);
251 for (int i = 0; i < 6; i++)
252 eFe[i] = robot_state.K_F_ext_hat_K[i];
253
254 // Transform in tool frame
256 vpForceTwistMatrix cWe(cMe);
257 force = cWe * eFe;
258 break;
259 }
260 default: {
261 throw(vpException(vpException::fatalError, "Cannot get Franka force/torque: frame not supported"));
262 }
263 }
264}
265
281{
282 if (!m_handler) {
283 throw(vpException(vpException::fatalError, "Cannot get Franka robot velocity: robot is not connected"));
284 }
285
286 franka::RobotState robot_state = getRobotInternalState();
287
288 switch (frame) {
289
290 case JOINT_STATE: {
291 d_position.resize(7);
292 for (int i = 0; i < 7; i++) {
293 d_position[i] = robot_state.dq[i];
294 }
295 break;
296 }
297
298 default: {
299 throw(vpException(vpException::fatalError, "Cannot get Franka cartesian velocity: not implemented"));
300 }
301 }
302}
303
310{
311 if (!m_handler) {
312 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
313 }
314
315 std::array<double, 7> coriolis_;
316
317 franka::RobotState robot_state = getRobotInternalState();
318
319 coriolis_ = m_model->coriolis(robot_state);
320
321 coriolis.resize(7);
322 for (int i = 0; i < 7; i++) {
323 coriolis[i] = coriolis_[i];
324 }
325}
326
332{
333 if (!m_handler) {
334 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
335 }
336
337 std::array<double, 7> gravity_;
338
339 franka::RobotState robot_state = getRobotInternalState();
340
341 gravity_ = m_model->gravity(robot_state);
342
343 gravity.resize(7);
344 for (int i = 0; i < 7; i++) {
345 gravity[i] = gravity_[i];
346 }
347}
348
354{
355 if (!m_handler) {
356 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
357 }
358
359 std::array<double, 49> mass_;
360
361 franka::RobotState robot_state = getRobotInternalState();
362
363 mass_ = m_model->mass(robot_state); // column-major
364
365 mass.resize(7, 7); // row-major
366 for (size_t i = 0; i < 7; i++) {
367 for (size_t j = 0; j < 7; j++) {
368 mass[i][j] = mass_[j * 7 + i];
369 }
370 }
371}
372
381{
382 if (!m_handler) {
383 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
384 }
385 if (q.size() != 7) {
386 throw(vpException(vpException::fatalError, "Joint position vector [%u] is not a 7-dim vector", q.size()));
387 }
388
389 std::array<double, 7> q_array;
390 for (size_t i = 0; i < 7; i++)
391 q_array[i] = q[i];
392
393 franka::RobotState robot_state = getRobotInternalState();
394
395 std::array<double, 16> pose_array =
396 m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
398 for (unsigned int i = 0; i < 4; i++) {
399 for (unsigned int j = 0; j < 4; j++) {
400 fMe[i][j] = pose_array[j * 4 + i];
401 }
402 }
403
404 return fMe;
405}
406
422{
424 return (fMe * m_eMc);
425}
426
437{
438 if (!m_handler) {
439 throw(vpException(vpException::fatalError, "Cannot get Franka robot position: robot is not connected"));
440 }
441 if (frame == JOINT_STATE) {
442 throw(vpException(vpException::fatalError, "Cannot get Franka joint position as a pose vector"));
443 }
444
445 franka::RobotState robot_state = getRobotInternalState();
446
447 std::array<double, 16> pose_array = robot_state.O_T_EE;
449 for (unsigned int i = 0; i < 4; i++) {
450 for (unsigned int j = 0; j < 4; j++) {
451 fMe[i][j] = pose_array[j * 4 + i];
452 }
453 }
454
455 switch (frame) {
456 case END_EFFECTOR_FRAME: {
457 pose.buildFrom(fMe);
458 break;
459 }
460 case TOOL_FRAME: {
461 pose.buildFrom(fMe * m_eMc);
462 break;
463 }
464 default: {
465 throw(vpException(vpException::fatalError, "Cannot get Franka cartesian position: not implemented"));
466 }
467 }
468}
469
476{
477 if (!m_handler) {
478 throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
479 }
480
481 franka::RobotState robot_state = getRobotInternalState();
482
483 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state); // column-major
484 eJe_.resize(6, 7); // row-major
485 for (size_t i = 0; i < 6; i++) {
486 for (size_t j = 0; j < 7; j++) {
487 eJe_[i][j] = jacobian[j * 6 + i];
488 }
489 }
490 // TODO check from vpRobot fJe and fJeAvailable
491}
492
499{
500 if (!m_handler) {
501 throw(vpException(vpException::fatalError, "Cannot get Franka robot eJe jacobian: robot is not connected"));
502 }
503
504 franka::RobotState robot_state = getRobotInternalState();
505
506 std::array<double, 7> q_array;
507 for (size_t i = 0; i < 7; i++)
508 q_array[i] = q[i];
509
510 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
511 robot_state.EE_T_K); // column-major
512 eJe_.resize(6, 7); // row-major
513 for (size_t i = 0; i < 6; i++) {
514 for (size_t j = 0; j < 7; j++) {
515 eJe_[i][j] = jacobian[j * 6 + i];
516 }
517 }
518 // TODO check from vpRobot fJe and fJeAvailable
519}
520
527{
528 if (!m_handler) {
529 throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
530 }
531
532 franka::RobotState robot_state = getRobotInternalState();
533
534 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state); // column-major
535 fJe_.resize(6, 7); // row-major
536 for (size_t i = 0; i < 6; i++) {
537 for (size_t j = 0; j < 7; j++) {
538 fJe_[i][j] = jacobian[j * 6 + i];
539 }
540 }
541 // TODO check from vpRobot fJe and fJeAvailable
542}
543
550{
551 if (!m_handler) {
552 throw(vpException(vpException::fatalError, "Cannot get Franka robot fJe jacobian: robot is not connected"));
553 }
554 if (q.size() != 7) {
555 throw(vpException(
557 "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
558 q.size()));
559 }
560
561 franka::RobotState robot_state = getRobotInternalState();
562
563 std::array<double, 7> q_array;
564 for (size_t i = 0; i < 7; i++)
565 q_array[i] = q[i];
566
567 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
568 robot_state.EE_T_K); // column-major
569 fJe_.resize(6, 7); // row-major
570 for (size_t i = 0; i < 6; i++) {
571 for (size_t j = 0; j < 7; j++) {
572 fJe_[i][j] = jacobian[j * 6 + i];
573 }
574 }
575 // TODO check from vpRobot fJe and fJeAvailable
576}
577
587void vpRobotFranka::setLogFolder(const std::string &folder)
588{
589 if (!folder.empty()) {
590 if (vpIoTools::checkDirectory(folder) == false) {
591 try {
593 m_log_folder = folder;
594 } catch (const vpException &e) {
595 std::string error;
596 error = "Unable to create Franka log folder: " + folder;
598 }
599 } else {
600 m_log_folder = folder;
601 }
602 }
603}
604
611{
612 if (!m_handler) {
613 throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
614 }
616 std::cout << "Robot was not in position-based control. "
617 "Modification of the robot state"
618 << std::endl;
620 }
621
622 if (frame == vpRobot::JOINT_STATE) {
623 double speed_factor = m_positioningVelocity / 100.;
624
625 std::array<double, 7> q_goal;
626 for (size_t i = 0; i < 7; i++) {
627 q_goal[i] = position[i];
628 }
629
630 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
631
632 int nbAttempts = 10;
633 for (int attempt = 1; attempt <= nbAttempts; attempt++) {
634 try {
635 m_handler->control(joint_pos_traj_generator);
636 break;
637 } catch (const franka::ControlException &e) {
638 std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl;
639 m_handler->automaticErrorRecovery();
640 if (attempt == nbAttempts)
641 throw;
642 }
643 }
644 } else {
646 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
647 }
648}
649
658void vpRobotFranka::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
659
667{
668 switch (newState) {
669 case vpRobot::STATE_STOP: {
670 // Start primitive STOP only if the current state is velocity or force/torque
672 // Stop the robot
673 m_velControlThreadStopAsked = true;
674 if (m_velControlThread.joinable()) {
675 m_velControlThread.join();
676 m_velControlThreadStopAsked = false;
677 m_velControlThreadIsRunning = false;
678 }
680 // Stop the robot
681 m_ftControlThreadStopAsked = true;
682 if (m_ftControlThread.joinable()) {
683 m_ftControlThread.join();
684 m_ftControlThreadStopAsked = false;
685 m_ftControlThreadIsRunning = false;
686 }
687 }
688 break;
689 }
692 std::cout << "Change the control mode from velocity to position control.\n";
693 // Stop the robot
694 m_velControlThreadStopAsked = true;
695 if (m_velControlThread.joinable()) {
696 m_velControlThread.join();
697 m_velControlThreadStopAsked = false;
698 m_velControlThreadIsRunning = false;
699 }
701 std::cout << "Change the control mode from force/torque to position control.\n";
702 // Stop the robot
703 m_ftControlThreadStopAsked = true;
704 if (m_ftControlThread.joinable()) {
705 m_ftControlThread.join();
706 m_ftControlThreadStopAsked = false;
707 m_ftControlThreadIsRunning = false;
708 }
709 }
710 break;
711 }
714 std::cout << "Change the control mode from stop to velocity control.\n";
716 std::cout << "Change the control mode from position to velocity control.\n";
718 std::cout << "Change the control mode from force/torque to velocity control.\n";
719 // Stop the robot
720 m_ftControlThreadStopAsked = true;
721 if (m_ftControlThread.joinable()) {
722 m_ftControlThread.join();
723 m_ftControlThreadStopAsked = false;
724 m_ftControlThreadIsRunning = false;
725 }
726 }
727 break;
728 }
731 std::cout << "Change the control mode from stop to force/torque control.\n";
733 std::cout << "Change the control mode from position to force/torque control.\n";
735 std::cout << "Change the control mode from velocity to force/torque control.\n";
736 // Stop the robot
737 m_velControlThreadStopAsked = true;
738 if (m_velControlThread.joinable()) {
739 m_velControlThread.join();
740 m_velControlThreadStopAsked = false;
741 m_velControlThreadIsRunning = false;
742 }
743 }
744 break;
745 }
746
747 default:
748 break;
749 }
750
751 return vpRobot::setRobotState(newState);
752}
753
808{
811 "Cannot send a velocity to the robot. "
812 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
813 }
814
815 switch (frame) {
816 // Saturation in joint space
817 case JOINT_STATE: {
818 if (vel.size() != 7) {
819 throw vpRobotException(vpRobotException::wrongStateError, "Joint velocity vector (%d) is not of size 7",
820 vel.size());
821 }
822
824
825 vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
826
827 for (size_t i = 0; i < m_dq_des.size(); i++) { // TODO create a function to convert
828 m_dq_des[i] = vel_sat[i];
829 }
830
831 break;
832 }
833
834 // Saturation in cartesian space
838 if (vel.size() != 6) {
839 throw vpRobotException(vpRobotException::wrongStateError, "Cartesian velocity vector (%d) is not of size 6",
840 vel.size());
841 }
842 vpColVector vel_max(6);
843
844 for (unsigned int i = 0; i < 3; i++)
845 vel_max[i] = getMaxTranslationVelocity();
846 for (unsigned int i = 3; i < 6; i++)
847 vel_max[i] = getMaxRotationVelocity();
848
849 m_v_cart_des = vpRobot::saturateVelocities(vel, vel_max, true);
850
851 break;
852 }
853
854 case vpRobot::MIXT_FRAME: {
855 throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
856 }
857 }
858
859 if (!m_velControlThreadIsRunning) {
860 m_velControlThreadIsRunning = true;
861 m_velControlThread =
862 std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
863 std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
864 std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
865 std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
866 }
867}
868
869/*
870 Apply a force/torque to the robot.
871
872 \param[in] frame : Control frame in which the force/torque is applied.
873
874 \param[in] ft : Force/torque vector. The size of this vector
875 is always 6 for a cartesian force/torque skew, and 7 for joint torques.
876
877 \param[in] filter_gain : Value in range [0:1] to filter the force/torque vector \e ft.
878 To diable the filter set filter_gain to 1.
879 \param[in] activate_pi_controller : Activate proportional and integral low level controller.
880 */
882 const double &filter_gain, const bool &activate_pi_controller)
883{
884 switch (frame) {
885 // Saturation in joint space
886 case JOINT_STATE: {
887 if (ft.size() != 7) {
888 throw vpRobotException(vpRobotException::wrongStateError, "Joint torques vector (%d) is not of size 7",
889 ft.size());
890 }
891
892 for (size_t i = 0; i < m_tau_J_des.size(); i++) { // TODO create a function to convert
893 m_tau_J_des[i] = ft[i];
894 }
895 // TODO: Introduce force/torque saturation
896
897 break;
898 }
899
900 // Saturation in cartesian space
904 if (ft.size() != 6) {
905 throw vpRobotException(vpRobotException::wrongStateError, "Cartesian force/torque vector (%d) is not of size 6",
906 ft.size());
907 }
908
909 m_ft_cart_des = ft;
910 // TODO: Introduce force/torque saturation
911
912 break;
913 }
914
915 case vpRobot::MIXT_FRAME: {
916 throw vpRobotException(vpRobotException::wrongStateError, "Velocity controller not supported");
917 }
918 }
919
920 if (!m_ftControlThreadIsRunning) {
921 getRobotInternalState(); // Update m_robot_state internally
922 m_ftControlThreadIsRunning = true;
923 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
924 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
925 std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
926 std::ref(m_mutex), filter_gain, activate_pi_controller);
927 }
928}
929
931{
932 if (!m_handler) {
933 throw(vpException(vpException::fatalError, "Cannot get Franka robot state: robot is not connected"));
934 }
935 franka::RobotState robot_state;
936
937 if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
938 robot_state = m_handler->readOnce();
939
940 std::lock_guard<std::mutex> lock(m_mutex);
941 m_robot_state = robot_state;
942 } else { // robot_state is updated in the velocity control thread
943 std::lock_guard<std::mutex> lock(m_mutex);
944 robot_state = m_robot_state;
945 }
946
947 return robot_state;
948}
949
956{
957 vpColVector q_min(m_q_min.size());
958 for (size_t i = 0; i < m_q_min.size(); i++)
959 q_min[i] = m_q_min[i];
960
961 return q_min;
962}
969{
970 vpColVector q_max(m_q_max.size());
971 for (size_t i = 0; i < m_q_max.size(); i++)
972 q_max[i] = m_q_max[i];
973
974 return q_max;
975}
976
988
1001void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc) { m_eMc = eMc; }
1002
1012void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1013{
1014 vpColVector q;
1015
1016 this->readPosFile(filename, q);
1018 this->setPositioningVelocity(velocity_percentage);
1020}
1021
1066bool vpRobotFranka::readPosFile(const std::string &filename, vpColVector &q)
1067{
1068 std::ifstream fd(filename.c_str(), std::ios::in);
1069
1070 if (!fd.is_open()) {
1071 return false;
1072 }
1073
1074 std::string line;
1075 std::string key("R:");
1076 std::string id("#PANDA - Joint position file");
1077 bool pos_found = false;
1078 int lineNum = 0;
1079 size_t njoints = 7;
1080
1081 q.resize(njoints);
1082
1083 while (std::getline(fd, line)) {
1084 lineNum++;
1085 if (lineNum == 1) {
1086 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1087 std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1088 return false;
1089 }
1090 }
1091 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1092 continue;
1093 }
1094 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1095 // check if there are at least njoint values in the line
1096 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1097 if (chain.size() < njoints + 1) // try to split with tab separator
1098 chain = vpIoTools::splitChain(line, std::string("\t"));
1099 if (chain.size() < njoints + 1)
1100 continue;
1101
1102 std::istringstream ss(line);
1103 std::string key_;
1104 ss >> key_;
1105 for (unsigned int i = 0; i < njoints; i++)
1106 ss >> q[i];
1107 pos_found = true;
1108 break;
1109 }
1110 }
1111
1112 // converts rotations from degrees into radians
1113 for (unsigned int i = 0; i < njoints; i++) {
1114 q[i] = vpMath::rad(q[i]);
1115 }
1116
1117 fd.close();
1118
1119 if (!pos_found) {
1120 std::cout << "Error: unable to find a position for Panda robot in " << filename << std::endl;
1121 return false;
1122 }
1123
1124 return true;
1125}
1126
1149bool vpRobotFranka::savePosFile(const std::string &filename, const vpColVector &q)
1150{
1151
1152 FILE *fd;
1153 fd = fopen(filename.c_str(), "w");
1154 if (fd == NULL)
1155 return false;
1156
1157 fprintf(fd, "#PANDA - Joint position file\n"
1158 "#\n"
1159 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1160 "# with joint positions q1 to q7 expressed in degrees\n"
1161 "#\n");
1162
1163 // Save positions in mm and deg
1164 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
1165 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]), vpMath::deg(q[6]));
1166
1167 fclose(fd);
1168 return (true);
1169}
1170
1186
1194{
1195 if (m_franka_address.empty()) {
1196 throw(vpException(vpException::fatalError, "Cannot perform franka gripper homing without ip address"));
1197 }
1198 if (m_gripper == NULL)
1199 m_gripper = new franka::Gripper(m_franka_address);
1200
1201 m_gripper->homing();
1202}
1203
1214{
1215 if (m_franka_address.empty()) {
1216 throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1217 }
1218 if (m_gripper == NULL)
1219 m_gripper = new franka::Gripper(m_franka_address);
1220
1221 // Check for the maximum grasping width.
1222 franka::GripperState gripper_state = m_gripper->readOnce();
1223
1224 if (gripper_state.max_width < width) {
1225 std::cout << "Finger width request is too large for the current fingers on the gripper."
1226 << "Maximum possible width is " << gripper_state.max_width << std::endl;
1227 return EXIT_FAILURE;
1228 }
1229
1230 m_gripper->move(width, 0.1);
1231 return EXIT_SUCCESS;
1232}
1233
1243
1253{
1254 if (m_franka_address.empty()) {
1255 throw(vpException(vpException::fatalError, "Cannot open franka gripper without ip address"));
1256 }
1257 if (m_gripper == NULL)
1258 m_gripper = new franka::Gripper(m_franka_address);
1259
1260 // Check for the maximum grasping width.
1261 franka::GripperState gripper_state = m_gripper->readOnce();
1262
1263 m_gripper->move(gripper_state.max_width, 0.1);
1264 return EXIT_SUCCESS;
1265}
1266
1274{
1275 if (m_franka_address.empty()) {
1276 throw(vpException(vpException::fatalError, "Cannot release franka gripper without ip address"));
1277 }
1278 if (m_gripper == NULL)
1279 m_gripper = new franka::Gripper(m_franka_address);
1280
1281 m_gripper->stop();
1282}
1283
1298int vpRobotFranka::gripperGrasp(double grasping_width, double force)
1299{
1300 if (m_gripper == NULL)
1301 m_gripper = new franka::Gripper(m_franka_address);
1302
1303 // Check for the maximum grasping width.
1304 franka::GripperState gripper_state = m_gripper->readOnce();
1305 std::cout << "Gripper max witdh: " << gripper_state.max_width << std::endl;
1306 if (gripper_state.max_width < grasping_width) {
1307 std::cout << "Object is too large for the current fingers on the gripper."
1308 << "Maximum possible width is " << gripper_state.max_width << std::endl;
1309 return EXIT_FAILURE;
1310 }
1311
1312 // Grasp the object.
1313 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1314 std::cout << "Failed to grasp object." << std::endl;
1315 return EXIT_FAILURE;
1316 }
1317
1318 return EXIT_SUCCESS;
1319}
1320
1321#elif !defined(VISP_BUILD_SHARED_LIBS)
1322// Work around to avoid warning: libvisp_robot.a(vpRobotFranka.cpp.o) has
1323// no symbols
1324void dummy_vpRobotFranka(){};
1325#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition vpException.h:78
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
static bool checkDirectory(const std::string &dirname)
static void makeDirectory(const std::string &dirname)
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of a pose vector and operations on poses.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
franka::RobotState getRobotInternalState()
void move(const std::string &filename, double velocity_percentage=10.)
bool savePosFile(const std::string &filename, const vpColVector &q)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpHomogeneousMatrix get_eMc() const
int gripperGrasp(double grasping_width, double force=60.)
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:57
int nDof
number of degrees of freedom
Definition vpRobot.h:100
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ JOINT_STATE
Definition vpRobot.h:78
@ TOOL_FRAME
Definition vpRobot.h:82
@ MIXT_FRAME
Definition vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
@ STATE_FORCE_TORQUE_CONTROL
Initialize the force/torque controller.
Definition vpRobot.h:67
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248