Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpRobotKinova.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for Kinova Jaco robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_JACOSDK
42 
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotException.h>
45 
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/robot/vpRobotKinova.h>
53 
58  : m_eMc(), m_plugin_location("./"), m_verbose(false), m_plugin_loaded(false), m_devices_count(0),
59  m_devices_list(NULL), m_active_device(-1), m_command_layer(CMD_LAYER_UNSET), m_command_layer_handle()
60 {
61  init();
62 }
63 
68 {
69  closePlugin();
70 
71  if (m_devices_list) {
72  delete[] m_devices_list;
73  }
74 }
75 
80 void vpRobotKinova::setDoF(unsigned int dof)
81 {
82  if (dof == 4 || dof == 6 || dof == 7) {
83  nDof = dof;
84  } else {
86  "Unsupported Kinova Jaco degrees of freedom: %d. Possible values are 4, 6 or 7.", dof));
87  }
88 }
89 
94 {
95  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
96  // that is set to identity by default in the constructor.
97 
100 
101  // Set here the robot degrees of freedom number
102  nDof = 6; // Jaco2 default dof = 6
103 
104  m_devices_list = new KinovaDevice[MAX_KINOVA_DEVICE];
105 }
106 
107 /*
108 
109  At least one of these function has to be implemented to control the robot with a
110  Cartesian velocity:
111  - get_eJe()
112  - get_fJe()
113 
114 */
115 
124 {
125  if (!m_plugin_loaded) {
126  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
127  }
128  if (!m_devices_count) {
129  throw(vpException(vpException::fatalError, "No Kinova robot found"));
130  }
131 
132  (void)eJe;
133  std::cout << "Not implemented ! " << std::endl;
134 }
135 
144 {
145  if (!m_plugin_loaded) {
146  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
147  }
148  if (!m_devices_count) {
149  throw(vpException(vpException::fatalError, "No Kinova robot found"));
150  }
151 
152  (void)fJe;
153  std::cout << "Not implemented ! " << std::endl;
154 }
155 
156 /*
157 
158  At least one of these function has to be implemented to control the robot:
159  - setCartVelocity()
160  - setJointVelocity()
161 
162 */
163 
179 {
180  if (v.size() != 6) {
182  "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
183  }
184 
185  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
186  vpColVector v_c; // This is the velocity that the robot is able to apply in the camera frame
187  vpColVector v_mix; // This is the velocity that the robot is able to apply in the mix frame
188  switch (frame) {
189  case vpRobot::CAMERA_FRAME: {
190  // We have to transform the requested velocity in the end-effector frame.
191  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
192  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
193  // a velocity twist from tool (or camera) frame into end-effector frame
194  vpVelocityTwistMatrix eVc(m_eMc); // GET IT FROM CAMERA EXTRINSIC CALIBRATION FILE
195 
196  // Input velocity is expressed in camera or tool frame
197  v_c = v;
198 
199  // Tranform velocity in end-effector
200  v_e = eVc * v_c;
201 
202  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
203  vpColVector p_e;
205  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
206  vpRotationMatrix bRe(bre);
207  vpMatrix bVe(6, 6, 0);
208  bVe.eye();
209  bVe.insert(bRe, 0, 0);
210  v_mix = bVe * v_e;
211 
212  TrajectoryPoint pointToSend;
213  pointToSend.InitStruct();
214  // We specify that this point will be used an angular (joint by joint) velocity vector
215  pointToSend.Position.Type = CARTESIAN_VELOCITY;
216  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
217 
218  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
219  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
220  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
221  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
222  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
223  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
224 
225  KinovaSetCartesianControl(); // Not sure that this function is useful here
226 
227  KinovaSendBasicTrajectory(pointToSend);
228  break;
229  }
230 
232  // Input velocity is expressed in end-effector
233  v_e = v;
234 
235  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
236  vpColVector p_e;
238  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
239  vpRotationMatrix bRe(bre);
240  vpMatrix bVe(6, 6, 0);
241  bVe.eye();
242  bVe.insert(bRe, 0, 0);
243  v_mix = bVe * v_e;
244 
245  TrajectoryPoint pointToSend;
246  pointToSend.InitStruct();
247  // We specify that this point will be used an angular (joint by joint) velocity vector
248  pointToSend.Position.Type = CARTESIAN_VELOCITY;
249  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
250 
251  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
252  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
253  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
254  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
255  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
256  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
257 
258  KinovaSetCartesianControl(); // Not sure that this function is useful here
259 
260  KinovaSendBasicTrajectory(pointToSend);
261  break;
262  }
263 
264  case vpRobot::MIXT_FRAME: {
265 
266  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
267  vpColVector p_e;
269  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
270  vpRotationMatrix bRe(bre);
271  std::cout << "rotation matrix from base to endeffector is bRe : " << std::endl;
272  std::cout << "bRe:\n" << bRe << std::endl;
273  vpMatrix bVe(6, 6, 0);
274  bVe.eye();
275  bVe.insert(bRe, 0, 0);
276  v_e = v;
277  // vpColVector bVe;
278  vpColVector v_mix = bVe * v_e;
279 
280  TrajectoryPoint pointToSend;
281  pointToSend.InitStruct();
282  // We specify that this point will be used an angular (joint by joint) velocity vector
283  pointToSend.Position.Type = CARTESIAN_VELOCITY;
284  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
285 
286  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
287  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
288  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
289  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_e[3]);
290  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_e[4]);
291  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_e[5]);
292 
293  KinovaSetCartesianControl(); // Not sure that this function is useful here
294  KinovaSendBasicTrajectory(pointToSend);
295  break;
296  }
299  // Out of the scope
300  break;
301  }
302 }
303 
309 {
310  if (qdot.size() != static_cast<unsigned int>(nDof)) {
312  "Cannot apply a %d-dim joint velocity vector to the Jaco robot configured with %d DoF",
313  qdot.size(), nDof));
314  }
315  TrajectoryPoint pointToSend;
316  pointToSend.InitStruct();
317  // We specify that this point will be used an angular (joint by joint) velocity vector
318  pointToSend.Position.Type = ANGULAR_VELOCITY;
319  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
320  switch (nDof) {
321  case 7: {
322  pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(qdot[6]));
323  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
324  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
325  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
326  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
327  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
328  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
329  break;
330  }
331  case 6: {
332  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
333  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
334  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
335  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
336  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
337  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
338  break;
339  }
340  case 4: {
341  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
342  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
343  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
344  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
345  break;
346  }
347  default:
348  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
349  }
350 
351  KinovaSetAngularControl(); // Not sure that this function is useful here
352 
353  KinovaSendBasicTrajectory(pointToSend);
354 }
355 
374 {
375  if (!m_plugin_loaded) {
376  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
377  }
378  if (!m_devices_count) {
379  throw(vpException(vpException::fatalError, "No Kinova robot found"));
380  }
383  "Cannot send a velocity to the robot. "
384  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
385  "entering your control loop.");
386  }
387 
388  vpColVector vel_sat(6);
389 
390  // Velocity saturation
391  switch (frame) {
392  // Saturation in cartesian space
393  case vpRobot::TOOL_FRAME:
396  case vpRobot::MIXT_FRAME: {
397  if (vel.size() != 6) {
399  "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
400  }
401  vpColVector vel_max(6);
402 
403  for (unsigned int i = 0; i < 3; i++)
404  vel_max[i] = getMaxTranslationVelocity();
405  for (unsigned int i = 3; i < 6; i++)
406  vel_max[i] = getMaxRotationVelocity();
407 
408  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
409 
410  setCartVelocity(frame, vel_sat);
411  break;
412  }
413 
414  // Saturation in joint space
415  case vpRobot::JOINT_STATE: {
416  if (vel.size() != static_cast<size_t>(nDof)) {
417  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)",
418  nDof, vel.size()));
419  }
420  vpColVector vel_max(vel.size());
421 
422  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
423  vel_max = getMaxRotationVelocity();
424 
425  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
426 
427  setJointVelocity(vel_sat);
428  }
429  }
430 }
431 
432 /*
433 
434  THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
435 
436 */
437 
446 {
447  AngularPosition currentCommand;
448 
449  // We get the actual angular command of the robot. Values are in deg
450  KinovaGetAngularCommand(currentCommand);
451 
452  q.resize(nDof);
453  switch (nDof) {
454  case 7: {
455  q[6] = vpMath::rad(currentCommand.Actuators.Actuator7);
456  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
457  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
458  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
459  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
460  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
461  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
462  break;
463  }
464  case 6: {
465  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
466  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
467  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
468  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
469  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
470  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
471  break;
472  }
473  case 4: {
474  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
475  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
476  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
477  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
478  break;
479  }
480  default:
481  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
482  }
483 }
484 
514 {
515  if (!m_plugin_loaded) {
516  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
517  }
518  if (!m_devices_count) {
519  throw(vpException(vpException::fatalError, "No Kinova robot found"));
520  }
521 
522  if (frame == JOINT_STATE) {
523  getJointPosition(position);
524  } else if (frame == END_EFFECTOR_FRAME) {
525  CartesianPosition currentCommand;
526  // We get the actual cartesian position of the robot
527  KinovaGetCartesianCommand(currentCommand);
528  position.resize(6);
529  position[0] = currentCommand.Coordinates.X;
530  position[1] = currentCommand.Coordinates.Y;
531  position[2] = currentCommand.Coordinates.Z;
532  position[3] = currentCommand.Coordinates.ThetaX;
533  position[4] = currentCommand.Coordinates.ThetaY;
534  position[5] = currentCommand.Coordinates.ThetaZ;
535  } else {
536  std::cout << "Not implemented ! " << std::endl;
537  }
538 }
539 
560 {
561  if (frame == JOINT_STATE) {
562  throw(vpException(vpException::fatalError, "Cannot get Jaco joint position as a pose vector"));
563  }
564 
565  vpColVector position;
566  getPosition(frame, position);
567 
568  vpRxyzVector rxyz; // reference frame to end-effector frame rotations
569  // Update the transformation between reference frame and end-effector frame
570  for (unsigned int i = 0; i < 3; i++) {
571  pose[i] = position[i]; // tx, ty, tz
572  rxyz[i] = position[i + 3]; // ry, ry, rz
573  }
574  vpThetaUVector tu(rxyz);
575  for (unsigned int i = 0; i < 3; i++) {
576  pose[i + 3] = tu[i]; // tux, tuy, tuz
577  }
578 }
579 
587 {
588  if (!m_plugin_loaded) {
589  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
590  }
591  if (!m_devices_count) {
592  throw(vpException(vpException::fatalError, "No Kinova robot found"));
593  }
594  if (frame == vpRobot::JOINT_STATE) {
595  if (static_cast<int>(q.size()) != nDof) {
597  "Cannot move robot to a joint position of dim %u that is not a %d-dim vector", q.size(), nDof));
598  }
599  TrajectoryPoint pointToSend;
600  pointToSend.InitStruct();
601  // We specify that this point will be an angular(joint by joint) position.
602  pointToSend.Position.Type = ANGULAR_POSITION;
603  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
604  switch (nDof) {
605  case 7: {
606  pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(q[6]));
607  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
608  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
609  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
610  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
611  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
612  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
613  break;
614  }
615  case 6: {
616  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
617  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
618  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
619  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
620  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
621  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
622  break;
623  }
624  case 4: {
625  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
626  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
627  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
628  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
629  break;
630  }
631  default:
632  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
633  }
634 
635  KinovaSetAngularControl(); // Not sure that this function is useful here
636 
637  if (m_verbose) {
638  std::cout << "Move robot to joint position [rad rad rad rad rad rad]: " << q.t() << std::endl;
639  }
640  KinovaSendBasicTrajectory(pointToSend);
641  } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
642  if (q.size() != 6) {
644  "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.size()));
645  }
646  TrajectoryPoint pointToSend;
647  pointToSend.InitStruct();
648  // We specify that this point will be an angular(joint by joint) position.
649  pointToSend.Position.Type = CARTESIAN_POSITION;
650  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
651  pointToSend.Position.CartesianPosition.X = static_cast<float>(q[0]);
652  pointToSend.Position.CartesianPosition.Y = static_cast<float>(q[1]);
653  pointToSend.Position.CartesianPosition.Z = static_cast<float>(q[2]);
654  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(q[3]);
655  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(q[4]);
656  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(q[5]);
657 
658  KinovaSetCartesianControl(); // Not sure that this function is useful here
659 
660  if (m_verbose) {
661  std::cout << "Move robot to cartesian position [m m m rad rad rad]: " << q.t() << std::endl;
662  }
663  KinovaSendBasicTrajectory(pointToSend);
664 
665  } else {
667  "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
668  }
669 }
670 
678 {
679  if (!m_plugin_loaded) {
680  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
681  }
682  if (!m_devices_count) {
683  throw(vpException(vpException::fatalError, "No Kinova robot found"));
684  }
685 
686  (void)frame;
687  (void)q;
688  std::cout << "Not implemented ! " << std::endl;
689 }
690 
703 {
705  throw(vpException(vpException::fatalError, "Kinova robot command layer unset"));
706  }
707  if (m_plugin_loaded) {
708  closePlugin();
709  }
710 #ifdef __linux__
711  // We load the API
712  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("Kinova.API.USBCommandLayerUbuntu.so")
713  : std::string("Kinova.API.EthCommandLayerUbuntu.so");
714  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
715  if (m_verbose) {
716  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
717  }
718  m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
719 
720  std::string prefix = (m_command_layer == CMD_LAYER_USB) ? std::string("") : std::string("Ethernet_");
721  // We load the functions from the library
722  KinovaCloseAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("CloseAPI")).c_str());
723  KinovaGetAngularCommand =
724  (int (*)(AngularPosition &))dlsym(m_command_layer_handle, (prefix + std::string("GetAngularCommand")).c_str());
725  KinovaGetCartesianCommand = (int (*)(CartesianPosition &))dlsym(
726  m_command_layer_handle, (prefix + std::string("GetCartesianCommand")).c_str());
727  KinovaGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result))dlsym(
728  m_command_layer_handle, (prefix + std::string("GetDevices")).c_str());
729  KinovaInitAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitAPI")).c_str());
730  KinovaInitFingers = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitFingers")).c_str());
731  KinovaMoveHome = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("MoveHome")).c_str());
732  KinovaSendBasicTrajectory =
733  (int (*)(TrajectoryPoint))dlsym(m_command_layer_handle, (prefix + std::string("SendBasicTrajectory")).c_str());
734  KinovaSetActiveDevice =
735  (int (*)(KinovaDevice devices))dlsym(m_command_layer_handle, (prefix + std::string("SetActiveDevice")).c_str());
736  KinovaSetAngularControl =
737  (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetAngularControl")).c_str());
738  KinovaSetCartesianControl =
739  (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetCartesianControl")).c_str());
740 #elif _WIN32
741  // We load the API.
742  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("CommandLayerWindows.dll")
743  : std::string("CommandLayerEthernet.dll");
744  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
745  if (m_verbose) {
746  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
747  }
748  m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
749 
750  // We load the functions from the library
751  KinovaCloseAPI = (int (*)())GetProcAddress(m_command_layer_handle, "CloseAPI");
752  KinovaGetAngularCommand = (int (*)(AngularPosition &))GetProcAddress(m_command_layer_handle, "GetAngularCommand");
753  KinovaGetCartesianCommand =
754  (int (*)(CartesianPosition &))GetProcAddress(m_command_layer_handle, "GetCartesianCommand");
755  KinovaGetDevices =
756  (int (*)(KinovaDevice[MAX_KINOVA_DEVICE], int &))GetProcAddress(m_command_layer_handle, "GetDevices");
757  KinovaInitAPI = (int (*)())GetProcAddress(m_command_layer_handle, "InitAPI");
758  KinovaInitFingers = (int (*)())GetProcAddress(m_command_layer_handle, "InitFingers");
759  KinovaMoveHome = (int (*)())GetProcAddress(m_command_layer_handle, "MoveHome");
760  KinovaSendBasicTrajectory = (int (*)(TrajectoryPoint))GetProcAddress(m_command_layer_handle, "SendBasicTrajectory");
761  KinovaSetActiveDevice = (int (*)(KinovaDevice))GetProcAddress(m_command_layer_handle, "SetActiveDevice");
762  KinovaSetAngularControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetAngularControl");
763  KinovaSetCartesianControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetCartesianControl");
764 #endif
765 
766  // Verify that all functions has been loaded correctly
767  if ((KinovaCloseAPI == NULL) || (KinovaGetAngularCommand == NULL) || (KinovaGetAngularCommand == NULL) ||
768  (KinovaGetCartesianCommand == NULL) || (KinovaGetDevices == NULL) || (KinovaInitAPI == NULL) ||
769  (KinovaInitFingers == NULL) || (KinovaMoveHome == NULL) || (KinovaSendBasicTrajectory == NULL) ||
770  (KinovaSetActiveDevice == NULL) || (KinovaSetAngularControl == NULL) || (KinovaSetCartesianControl == NULL)) {
771  throw(vpException(vpException::fatalError, "Cannot load plugin from \"%s\" folder", m_plugin_location.c_str()));
772  }
773  if (m_verbose) {
774  std::cout << "Plugin successfully loaded" << std::endl;
775  }
776 
777  m_plugin_loaded = true;
778 }
779 
784 {
785  if (m_plugin_loaded) {
786  if (m_verbose) {
787  std::cout << "Close plugin" << std::endl;
788  }
789 #ifdef __linux__
790  dlclose(m_command_layer_handle);
791 #elif _WIN32
792  FreeLibrary(m_command_layer_handle);
793 #endif
794  m_plugin_loaded = false;
795  }
796 }
797 
802 {
803  if (!m_plugin_loaded) {
804  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
805  }
806  if (!m_devices_count) {
807  throw(vpException(vpException::fatalError, "No Kinova robot found"));
808  }
809 
810  if (m_verbose) {
811  std::cout << "Move the robot to home position" << std::endl;
812  }
813  KinovaMoveHome();
814 }
815 
821 {
822  loadPlugin();
823  if (!m_plugin_loaded) {
824  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
825  }
826 
827  int result = (*KinovaInitAPI)();
828 
829  if (m_verbose) {
830  std::cout << "Initialization's result: " << result << std::endl;
831  }
832 
833  m_devices_count = KinovaGetDevices(m_devices_list, result);
834 
835  if (m_verbose) {
836  std::cout << "Found " << m_devices_count << " devices" << std::endl;
837  }
838 
839  // By default set the first device as active
840  setActiveDevice(0);
841 
842  // Initialize fingers
843  KinovaInitFingers();
844 
845  return m_devices_count;
846 }
847 
859 {
860  if (!m_plugin_loaded) {
861  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
862  }
863  if (!m_devices_count) {
864  throw(vpException(vpException::fatalError, "No Kinova robot found"));
865  }
866  if (device < 0 || device >= m_devices_count) {
867  throw(vpException(vpException::badValue, "Cannot set Kinova active device %d. Value should be in range [0, %d]",
868  device, m_devices_count - 1));
869  }
870  if (device != m_active_device) {
871  m_active_device = device;
872  KinovaSetActiveDevice(m_devices_list[m_active_device]);
873  }
874 }
875 
876 #elif !defined(VISP_BUILD_SHARED_LIBS)
877 // Work around to avoid warning: libvisp_robot.a(vpRobotKinova.cpp.o) has
878 // no symbols
879 void dummy_vpRobotKinova(){};
880 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:294
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1764
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:154
void eye()
Definition: vpMatrix.cpp:447
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5970
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:194
Error that can be emited by the vpRobot class and its derivates.
KinovaDevice * m_devices_list
void get_eJe(vpMatrix &eJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
virtual ~vpRobotKinova()
void setDoF(unsigned int dof)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
std::string m_plugin_location
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
CommandLayer m_command_layer
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setJointVelocity(const vpColVector &qdot)
void getJointPosition(vpColVector &q)
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setActiveDevice(int device)
int nDof
number of degrees of freedom
Definition: vpRobot.h:103
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:98
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:105
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ JOINT_STATE
Definition: vpRobot.h:81
@ TOOL_FRAME
Definition: vpRobot.h:85
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
static const double maxRotationVelocityDefault
Definition: vpRobot.h:100
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:109
double maxTranslationVelocity
Definition: vpRobot.h:97
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:99
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
Implementation of a rotation vector as axis-angle minimal representation.