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
51 /*!
52
53 Default constructor.
54
55 */
vpRobotFranka()56 vpRobotFranka::vpRobotFranka()
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
68 /*!
69 * Establishes a connection with the robot.
70 * \param[in] franka_address IP/hostname of the robot.
71 * \param[in] realtime_config If set to kEnforce, an exception will be thrown if realtime priority cannot
72 * be set when required. Setting realtime_config to kIgnore disables this behavior.
73 */
vpRobotFranka(const std::string & franka_address,franka::RealtimeConfig realtime_config)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
87 /*!
88 * Initialize internal vars, such as min, max joint positions, max velocity and acceleration.
89 */
init()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
100 /*!
101
102 Destructor.
103
104 */
~vpRobotFranka()105 vpRobotFranka::~vpRobotFranka()
106 {
107 setRobotState(vpRobot::STATE_STOP);
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
123 /*!
124 * Establishes a connection with the robot and set default behavior.
125 * \param[in] franka_address IP/hostname of the robot.
126 * \param[in] realtime_config If set to kEnforce, an exception will be thrown if realtime priority cannot
127 * be set when required. Setting realtime_config to kIgnore disables this behavior.
128 */
connect(const std::string & franka_address,franka::RealtimeConfig realtime_config)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
173 /*!
174 * Get robot position.
175 * \param[in] frame : Type of position to retrieve. Admissible values are:
176 * - vpRobot::JOINT_STATE to get the 7 joint positions.
177 * - vpRobot::END_EFFECTOR_FRAME to retrieve the cartesian position of the end-effector frame wrt the robot base frame.
178 * - vpRobot::CAMERA_FRAME to retrieve the cartesian position of the camera frame (or more generally a tool frame
179 * vpRobot::TOOL_FRAME) wrt the robot base frame.
180 * \param[out] position : Robot position. When joint position is asked this vector is 7-dim. Otherwise for a cartesian
181 * position this vector is 6-dim. Its content is similar to a vpPoseVector, with first the 3 tranlations in meter
182 * and then the 3 orientations in radian as a \f$\theta {\bf u}\f$ vector (see vpThetaUVector).
183 *
184 * If you want to get a cartesian position, use rather
185 * getPosition(const vpRobot::vpControlFrameType, vpPoseVector &)
186 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & position)187 void vpRobotFranka::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
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
228 /*!
229 * Get robot force torque.
230 * \param[in] frame : Type of forces and torques to retrieve. Admissible values are:
231 * - vpRobot::JOINT_STATE to get the 7-dim measured link-side joint torque sensor signals. Unit: \f$[Nm]\f$.
232 * - vpRobot::END_EFFECTOR_FRAME to retrieve the external wrench (force, torque) acting on stiffness frame, expressed relative to the stiffness
233 * frame. Unit: \f$[N,N,N,Nm,Nm,Nm]\f$.
234 * - vpRobot::CAMERA_FRAME or more generally a tool frame vpRobot::TOOL_FRAME to retrieve the external wrench (force, torque) applied on the tool frame.
235 * Unit: \f$[N,N,N,Nm,Nm,Nm]\f$.
236 * \param[out] force : Measured forced and torques.
237 *
238 * If you want to get a cartesian position, use rather
239 * getPosition(const vpRobot::vpControlFrameType, vpPoseVector &)
240 */
getForceTorque(const vpRobot::vpControlFrameType frame,vpColVector & force)241 void vpRobotFranka::getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
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
270 vpHomogeneousMatrix cMe = get_eMc().inverse();
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
281 /*!
282 * Get robot velocity.
283 * \param[in] frame : Type of velocity to retrieve. Admissible values are:
284 * - vpRobot::JOINT_STATE to get the 7 joint positions.
285 * - vpRobot::END_EFFECTOR_FRAME to retrieve the cartesian velocity of the end-effector frame wrt the robot base frame.
286 * - vpRobot::CAMERA_FRAME to retrieve the cartesian velocity of the camera frame (or more generally a tool frame
287 * vpRobot::TOOL_FRAME) wrt the robot base frame.
288 * \param[out] d_position : Robot velocity. When joints velocity is asked this vector is 7-dim. Otherwise for a cartesian
289 * velocity this vector is 6-dim. Its content is similar to a vpPoseVector, with first the 3 tranlations in meter
290 * and then the 3 orientations in radian as a \f$\theta {\bf u}\f$ vector (see vpThetaUVector).
291 *
292 * \warning For the moment, cartesian velocities measurement in vpRobot::END_EFFECTOR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::TOOL_FRAME
293 * are not implemented.
294 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & d_position)295 void vpRobotFranka::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
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
319 /*!
320 * Get the Coriolis force vector (state-space equation) calculated from the current robot state: \f$ c= C \times
321 * dq\f$, in \f$[Nm]\f$.
322 * \param[out] coriolis : Coriolis force vector.
323 */
getCoriolis(vpColVector & coriolis)324 void vpRobotFranka::getCoriolis(vpColVector &coriolis)
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
342 /*!
343 * Get the gravity vector calculated form the current robot state. Unit: \f$[Nm]\f$.
344 * \param[out] gravity : Gravity vector
345 */
getGravity(vpColVector & gravity)346 void vpRobotFranka::getGravity(vpColVector &gravity)
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
364 /*!
365 * Get the 7x7 mass matrix. Unit: \f$[kg \times m^2]\f$.
366 * \param[out] mass : 7x7 mass matrix, row-major.
367 */
getMass(vpMatrix & mass)368 void vpRobotFranka::getMass(vpMatrix &mass)
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
388 /*!
389 * Given the joint position of the robot, computes the forward kinematics (direct geometric model) as an
390 * homogeneous matrix \f${^f}{\bf M}_e\f$ that gives the position of the end-effector in the robot base frame.
391 *
392 * \param[in] q : Joint position as a 7-dim vector.
393 * \return Position of the end-effector in the robot base frame.
394 */
get_fMe(const vpColVector & q)395 vpHomogeneousMatrix vpRobotFranka::get_fMe(const vpColVector &q)
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);
411 vpHomogeneousMatrix fMe;
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
421 /*!
422 * Given the joint position of the robot, computes the forward kinematics (direct geometric model) as an
423 * homogeneous matrix \f${^f}{\bf M}_c\f$ that gives the position of the camera frame (or in general of
424 * any tool attached to the robot) in the robot base frame.
425 *
426 * By default, the transformation \f$^{e}{\bf M}_c\f$ that corresponds to the transformation between
427 * the end-effector and the camera (or tool) frame is set to identity, meaning that the camera (or tool)
428 * frame is located on the end-effector.
429 *
430 * To change the position of the camera (or tool) frame , use set_eMc().
431 *
432 * \param[in] q : Joint position as a 7-dim vector.
433 * \return Position of the camera frame (or tool frame) in the robot base frame.
434 */
get_fMc(const vpColVector & q)435 vpHomogeneousMatrix vpRobotFranka::get_fMc(const vpColVector &q)
436 {
437 vpHomogeneousMatrix fMe = get_fMe(q);
438 return (fMe * m_eMc);
439 }
440
441 /*!
442 * Get robot cartesian position.
443 * \param[in] frame : Type of cartesian position to retrieve. Admissible values are:
444 * - vpRobot::END_EFFECTOR_FRAME to retrieve the cartesian position of the end-effector frame wrt the robot base frame.
445 * - vpRobot::CAMERA_FRAME to retrieve the cartesian position of the camera frame (or more generally a tool frame
446 * vpRobot::TOOL_FRAME) wrt the robot base frame.
447 * \param[out] pose : Robot cartesian position. This vector is 6-dim. Its content is similar to a
448 * vpPoseVector, with first the 3 tranlations in meter and then the 3 orientations in radian as a
449 * \f$\theta {\bf u}\f$ vector (see vpThetaUVector).
450 */
getPosition(const vpRobot::vpControlFrameType frame,vpPoseVector & pose)451 void vpRobotFranka::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &pose)
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;
463 vpHomogeneousMatrix fMe;
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
485 /*!
486 * Gets the robot Jacobian in the end-effector frame relative to the end-effector frame represented as a 6x7 matrix in row-major
487 * format and computed from the robot current joint position.
488 * \param[out] eJe_ : Body Jacobian expressed in the end-effector frame.
489 */
get_eJe(vpMatrix & eJe_)490 void vpRobotFranka::get_eJe(vpMatrix &eJe_)
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
508 /*!
509 * Gets the robot Jacobian in the end-effector frame relative to the end-effector frame represented as a 6x7 matrix in row-major
510 * format and computed from the robot current joint position.
511 * \param[in] q : 7-dim vector corresponding to the robot joint position [rad].
512 * \param[out] eJe_ : Body Jacobian expressed in the end-effector frame.
513 */
get_eJe(const vpColVector & q,vpMatrix & eJe_)514 void vpRobotFranka::get_eJe(const vpColVector &q, vpMatrix &eJe_)
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
537 /*!
538 * Gets the robot Jacobian in the end-effector frame relative to the base frame represented as a 6x7 matrix in row-major format and computed
539 * from the robot current joint position.
540 * \param[out] fJe_ : Zero Jacobian expressed in the base frame.
541 */
get_fJe(vpMatrix & fJe_)542 void vpRobotFranka::get_fJe(vpMatrix &fJe_)
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
560 /*!
561 * Gets the robot Jacobian in the end-effector frame relative to the base frame represented as a 6x7 matrix in row-major format and computed
562 * from the robot joint position given as input.
563 * \param[in] q : 7-dim vector corresponding to the robot joint position [rad].
564 * \param[out] fJe_ : Zero Jacobian expressed in the base frame.
565 */
get_fJe(const vpColVector & q,vpMatrix & fJe_)566 void vpRobotFranka::get_fJe(const vpColVector &q, vpMatrix &fJe_)
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
591 /*!
592 * Set the folder or directory used to record logs at 1Kz when setVelocity() is used.
593 * By default the log folder is empty.
594 *
595 * When the log folder is empty, logs are not created.
596 *
597 * \param[in] folder : A path to a folder that will contain a basket of log files. If the folder doesn't exist
598 * it will be created recursively.
599 */
setLogFolder(const std::string & folder)600 void vpRobotFranka::setLogFolder(const std::string &folder)
601 {
602 if (!folder.empty()) {
603 if (vpIoTools::checkDirectory(folder) == false) {
604 try {
605 vpIoTools::makeDirectory(folder);
606 m_log_folder = folder;
607 }
608 catch(const vpException &e) {
609 std::string error;
610 error = "Unable to create Franka log folder: " + folder;
611 throw(vpException(vpException::fatalError, error));
612 }
613 }
614 else {
615 m_log_folder = folder;
616 }
617 }
618 }
619
620 /*!
621 * Set robot position. This function is blocking; it returns when the desired position is reached.
622 * \param[in] frame : The only possible value is vpRobot::JOINT_STATE. Other values are not implemented.
623 * \param[in] position : This is a 7-dim vector that corresponds to the robot joint positions expressed in rad.
624 */
setPosition(const vpRobot::vpControlFrameType frame,const vpColVector & position)625 void vpRobotFranka::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
626 {
627 if (!m_handler) {
628 throw(vpException(vpException::fatalError, "Cannot set Franka robot position: robot is not connected"));
629 }
630 if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
631 std::cout << "Robot was not in position-based control. "
632 "Modification of the robot state" << std::endl;
633 setRobotState(vpRobot::STATE_POSITION_CONTROL);
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 {
660 throw (vpException(vpRobotException::functionNotImplementedError,
661 "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
662 }
663 }
664
665 /*!
666
667 Set the maximal velocity percentage to use for a position control.
668
669 \param[in] velocity : Percentage of the maximal velocity. Values should
670 be in ]0:100].
671
672 */
setPositioningVelocity(double velocity)673 void vpRobotFranka::setPositioningVelocity(double velocity)
674 {
675 m_positionningVelocity = velocity;
676 }
677
678 /*!
679
680 Change the robot state.
681
682 \param[in] newState : New requested robot state.
683 */
setRobotState(vpRobot::vpRobotStateType newState)684 vpRobot::vpRobotStateType vpRobotFranka::setRobotState(vpRobot::vpRobotStateType newState)
685 {
686 switch (newState) {
687 case vpRobot::STATE_STOP: {
688 // Start primitive STOP only if the current state is velocity or force/torque
689 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
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 }
698 else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
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 }
709 case vpRobot::STATE_POSITION_CONTROL: {
710 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
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 }
720 else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
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 }
732 case vpRobot::STATE_VELOCITY_CONTROL: {
733 if (vpRobot::STATE_STOP == getRobotState()) {
734 std::cout << "Change the control mode from stop to velocity control.\n";
735 }
736 else if (vpRobot::STATE_POSITION_CONTROL == getRobotState()) {
737 std::cout << "Change the control mode from position to velocity control.\n";
738 }
739 else if (vpRobot::STATE_FORCE_TORQUE_CONTROL == getRobotState()) {
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 }
751 case vpRobot::STATE_FORCE_TORQUE_CONTROL: {
752 if (vpRobot::STATE_STOP == getRobotState()) {
753 std::cout << "Change the control mode from stop to force/torque control.\n";
754 }
755 else if (vpRobot::STATE_POSITION_CONTROL == getRobotState()) {
756 std::cout << "Change the control mode from position to force/torque control.\n";
757 }
758 else if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
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
778 /*!
779 Apply a velocity to the robot.
780
781 \param[in] frame : Control frame in which the velocity is expressed. Velocities
782 could be expressed as joint velocities, cartesian velocity twist expressed in
783 the robot reference frame, in the end-effector frame or in the camera or tool
784 frame.
785
786 \param[in] vel : Velocity vector. Translation velocities are expressed
787 in m/s while rotation velocities in rad/s. The size of this vector
788 is always 6 for a cartesian velocity skew, and 7 for joint velocities.
789
790 - When joint velocities have to be applied, frame should be set to vpRobot::JOINT_STATE,
791 and \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
792 \dot{q}_5, \dot{q}_6]^t, \dot{q}_7]^T \f$ correspond to joint velocities in rad/s.
793
794 - When cartesian velocities have to be applied in the reference frame (or in a frame
795 also called fixed frame in ViSP), frame should be set to vpRobot::REFERENCE_FRAME,
796 \f$ vel = [^{f} v_x, ^{f} v_y, ^{f} v_z, ^{f}
797 \omega_x, ^{f} \omega_y, ^{f} \omega_z]^T \f$ is a velocity twist vector corresponding
798 to the velocity of the origin of the camera frame (or tool frame)
799 expressed in the reference frame, with translations velocities \f$ ^{f} v_x,
800 ^{f} v_y, ^{f} v_z \f$ in m/s and rotation velocities \f$ ^{f}\omega_x, ^{f}
801 \omega_y, ^{f} \omega_z \f$ in rad/s.
802
803 - When cartesian velocities have to be applied in the end-effector frame,
804 frame should be set to vpRobot::END_EFFECTOR_FRAME,
805 \f$ vel = [^{e} v_x, ^{e} v_y, ^{e} v_z, ^{e} \omega_x, ^{e} \omega_y,
806 ^{e} \omega_z]^T \f$ is a velocity twist vector corresponding
807 to the velocity of the origin of the end-effector frame
808 expressed in the end-effector frame, with translations velocities \f$ ^{e} v_x,
809 ^{e} v_y, ^{e} v_z \f$ in m/s and rotation velocities \f$ ^{e}\omega_x, ^{e}
810 \omega_y, ^{e} \omega_z \f$ in rad/s.
811
812 - When cartesian velocities have to be applied in the camera frame or more
813 generally in a tool frame, frame should be set to vpRobot::CAMERA_FRAME or vpRobot::TOOL_FRAME,
814 \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c} \omega_x, ^{c} \omega_y,
815 ^{c} \omega_z]^T \f$ is a velocity twist vector corresponding
816 to the velocity of the origin of the camera (or tool frame) frame
817 expressed in the camera (or tool frame), with translations velocities \f$ ^{c} v_x,
818 ^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
819 \omega_y, ^{c} \omega_z \f$ in rad/s.
820
821 \exception vpRobotException::wrongStateError : If a the robot is not
822 configured to handle a velocity. The robot can handle a velocity only if the
823 velocity control mode is set. For that, call setRobotState(
824 vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
825
826 \warning Velocities could be saturated if one of them exceed the
827 maximal autorized speed (see vpRobot::maxTranslationVelocity and
828 vpRobot::maxRotationVelocity). To change these values use
829 setMaxTranslationVelocity() and setMaxRotationVelocity().
830 */
setVelocity(const vpRobot::vpControlFrameType frame,const vpColVector & vel)831 void vpRobotFranka::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
832 {
833 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
834 throw vpRobotException(vpRobotException::wrongStateError,
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) {
843 throw vpRobotException(vpRobotException::wrongStateError,
844 "Joint velocity vector (%d) is not of size 7", vel.size());
845 }
846
847 vpColVector vel_max(7, getMaxRotationVelocity());
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
859 case vpRobot::TOOL_FRAME:
860 case vpRobot::REFERENCE_FRAME:
861 case vpRobot::END_EFFECTOR_FRAME: {
862 if (vel.size() != 6) {
863 throw vpRobotException(vpRobotException::wrongStateError,
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: {
879 throw vpRobotException(vpRobotException::wrongStateError,
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 */
setForceTorque(const vpRobot::vpControlFrameType frame,const vpColVector & ft,const double & filter_gain,const bool & activate_pi_controller)906 void vpRobotFranka::setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft,
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) {
913 throw vpRobotException(vpRobotException::wrongStateError,
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
926 case vpRobot::TOOL_FRAME:
927 case vpRobot::REFERENCE_FRAME:
928 case vpRobot::END_EFFECTOR_FRAME: {
929 if (ft.size() != 6) {
930 throw vpRobotException(vpRobotException::wrongStateError,
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: {
941 throw vpRobotException(vpRobotException::wrongStateError,
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
getRobotInternalState()956 franka::RobotState vpRobotFranka::getRobotInternalState()
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
977 /*!
978 Gets minimal joint values.
979 \return A 7-dimension vector that contains the minimal joint values for the 7 dof.
980 All the values are expressed in radians.
981 */
getJointMin() const982 vpColVector vpRobotFranka::getJointMin() const
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 }
990 /*!
991 Gets maximum joint values.
992 \return A 7-dimension vector that contains the maximum joint values for the 7 dof.
993 All the values are expressed in radians.
994 */
getJointMax() const995 vpColVector vpRobotFranka::getJointMax() const
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
1004 /*!
1005 * Return the \f$ ^{e}{\bf M}_c\f$ homogeneous transformation that gives the position
1006 * of the camera frame (or in general of any tool frame) in the robot end-effector frame.
1007 *
1008 * By default, this transformation is set to identity, meaning that the camera (or tool)
1009 * frame is located on the end-effector.
1010 *
1011 * To change the position of the camera (or tool) frame on the end-effector frame, use set_eMc().
1012
1013 */
get_eMc() const1014 vpHomogeneousMatrix vpRobotFranka::get_eMc() const
1015 {
1016 return m_eMc;
1017 }
1018
1019 /*!
1020 * Set the \f$ ^{e}{\bf M}_c\f$ homogeneous transformation that gives the position
1021 * of the camera frame (or in general of any tool frame) in the robot end-effector frame.
1022 *
1023 * By default, this transformation is set to identity, meaning that the camera (or tool)
1024 * frame is located on the end-effector.
1025 *
1026 * This transformation has to be set before controlling the robot cartesian velocity in
1027 * the camera frame or getting the position of the robot in the camera frame.
1028 *
1029 * \param[in] eMc : End-effector to camera frame transformation.
1030 */
set_eMc(const vpHomogeneousMatrix & eMc)1031 void vpRobotFranka::set_eMc(const vpHomogeneousMatrix &eMc)
1032 {
1033 m_eMc = eMc;
1034 }
1035
1036 /*!
1037
1038 Moves the robot to the joint position specified in the filename. The
1039 positioning velocity is set to 10% of the robot maximal velocity.
1040
1041 \param filename : File containing a joint position to reach.
1042 \param velocity_percentage : Velocity percentage. Values in range [1, 100].
1043
1044 */
move(const std::string & filename,double velocity_percentage)1045 void vpRobotFranka::move(const std::string &filename, double velocity_percentage)
1046 {
1047 vpColVector q;
1048
1049 this->readPosFile(filename, q);
1050 this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
1051 this->setPositioningVelocity(velocity_percentage);
1052 this->setPosition(vpRobot::JOINT_STATE, q);
1053 }
1054
1055 /*!
1056
1057 Read joint positions in a specific Franka position file.
1058
1059 This position file has to start with a header. The seven joint positions
1060 are given after the "R:" keyword and are expressed in degres to be more
1061 representative for the user. Theses values are then converted in
1062 radians in \e q. The character "#" starting a line indicates a
1063 comment.
1064
1065 A typical content of such a file is given below:
1066
1067 \code
1068 #PANDA - Joint position file
1069 #
1070 # R: q1 q2 q3 q4 q5 q6 q7
1071 # with joint positions q1 to q7 expressed in degrees
1072 #
1073
1074 R: 0.1 0.3 -0.25 -80.5 80 0 0
1075 \endcode
1076
1077 \param[in] filename : Name of the position file to read.
1078
1079 \param[out] q : 7-dim joint positions: q1 q2 q3 q4 q5 q6 q7 with values expressed in radians.
1080
1081 \return true if the positions were successfully readen in the file. false, if
1082 an error occurs.
1083
1084 The code below shows how to read a position from a file and move the robot to
1085 this position.
1086 \code
1087 vpRobotFranka robot;
1088 vpColVector q; // Joint position
1089 robot.readPosFile("myposition.pos", q); // Set the joint position from the file
1090 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1091
1092 robot.setPositioningVelocity(5); // Positioning velocity set to 5%
1093 robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
1094 \endcode
1095
1096 \sa savePosFile(), move()
1097 */
1098
readPosFile(const std::string & filename,vpColVector & q)1099 bool 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
1160 /*!
1161
1162 Save joint positions in a specific Panda position file.
1163
1164 This position file starts with a header on the first line. After
1165 convertion of the rotations in degrees, the joint position \e q is
1166 written on a line starting with the keyword "R: ". See readPosFile()
1167 documentation for an example of such a file.
1168
1169 \param filename : Name of the position file to create.
1170
1171 \param q : Joint positions vector to save in the
1172 filename with values expressed in radians.
1173
1174 \warning The joint rotations written in the file are converted
1175 in degrees to be more representative for the user.
1176
1177 \return true if the positions were successfully saved in the file. false, if
1178 an error occurs.
1179
1180 \sa readPosFile()
1181 */
savePosFile(const std::string & filename,const vpColVector & q)1182 bool 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
1205 /*!
1206
1207 Stop the robot when it is controlled in velocity and set the robot state to vpRobot::STATE_STOP.
1208
1209 \exception vpRobotException::lowLevelError : If the low level
1210 controller returns an error during robot stopping.
1211 */
stopMotion()1212 void vpRobotFranka::stopMotion()
1213 {
1214 if (getRobotState() == vpRobot::STATE_VELOCITY_CONTROL) {
1215 vpColVector q(7, 0);
1216 setVelocity(vpRobot::JOINT_STATE, q);
1217 }
1218 setRobotState(vpRobot::STATE_STOP);
1219 }
1220
1221 /*!
1222
1223 Performing a gripper homing.
1224
1225 \sa gripperGrasp(), gripperMove(), gripperRelease()
1226 */
gripperHoming()1227 void vpRobotFranka::gripperHoming()
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
1238 /*!
1239
1240 Moves the gripper fingers to a specified width.
1241 @param[in] width : Intended opening width. [m]
1242
1243 \return EXIT_SUCCESS if the success, EXIT_FAILURE otherwise.
1244
1245 \sa gripperHoming(), gripperGrasp(), gripperRelease()
1246 */
gripperMove(double width)1247 int vpRobotFranka::gripperMove(double width)
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
1268 /*!
1269
1270 Closes the gripper.
1271
1272 \return EXIT_SUCCESS if the success, EXIT_FAILURE otherwise.
1273
1274 \sa gripperHoming(), gripperGrasp(), gripperRelease(), gripperOpen()
1275 */
gripperClose()1276 int vpRobotFranka::gripperClose()
1277 {
1278 return gripperMove(0);
1279 }
1280
1281 /*!
1282
1283 Closes the gripper.
1284
1285 \return EXIT_SUCCESS if the success, EXIT_FAILURE otherwise.
1286
1287 \sa gripperHoming(), gripperGrasp(), gripperRelease(), gripperOpen()
1288 */
gripperOpen()1289 int vpRobotFranka::gripperOpen()
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
1304 /*!
1305
1306 Release an object that is grasped.
1307
1308 \sa gripperHoming(), gripperMove(), gripperRelease()
1309 */
gripperRelease()1310 void vpRobotFranka::gripperRelease()
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
1321 /*!
1322
1323 Grasp an object that has a given width.
1324
1325 An object is considered grasped if the distance \e d between the gripper fingers satisfies
1326 \e grasping_width - 0.005 < d < \e grasping_width + 0.005.
1327
1328 \param[in] grasping_width : Size of the object to grasp. [m]
1329 \param[in] force : Grasping force. [N]
1330
1331 \return EXIT_SUCCESS if grasping succeed, EXIT_FAILURE otherwise.
1332
1333 \sa gripperHoming(), gripperOpen(), gripperRelease()
1334 */
gripperGrasp(double grasping_width,double force)1335 int 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
dummy_vpRobotFranka()1361 void dummy_vpRobotFranka(){};
1362 #endif
1363
1364