Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpRobotKinova.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 Kinova Jaco robot.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_JACOSDK
39 
40 #include <visp3/core/vpIoTools.h>
41 #include <visp3/robot/vpRobotException.h>
42 
48 #include <visp3/core/vpHomogeneousMatrix.h>
49 #include <visp3/robot/vpRobotKinova.h>
50 
55  : m_eMc(), m_plugin_location("./"), m_verbose(false), m_plugin_loaded(false), m_devices_count(0),
56  m_devices_list(nullptr), m_active_device(-1), m_command_layer(CMD_LAYER_UNSET), m_command_layer_handle()
57 {
58  init();
59 }
60 
65 {
66  closePlugin();
67 
68  if (m_devices_list) {
69  delete[] m_devices_list;
70  }
71 }
72 
77 void vpRobotKinova::setDoF(unsigned int dof)
78 {
79  if (dof == 4 || dof == 6 || dof == 7) {
80  nDof = dof;
81  } else {
83  "Unsupported Kinova Jaco degrees of freedom: %d. Possible values are 4, 6 or 7.", dof));
84  }
85 }
86 
91 {
92  // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
93  // that is set to identity by default in the constructor.
94 
97 
98  // Set here the robot degrees of freedom number
99  nDof = 6; // Jaco2 default dof = 6
100 
101  m_devices_list = new KinovaDevice[MAX_KINOVA_DEVICE];
102 }
103 
104 /*
105 
106  At least one of these function has to be implemented to control the robot with a
107  Cartesian velocity:
108  - get_eJe()
109  - get_fJe()
110 
111 */
112 
121 {
122  if (!m_plugin_loaded) {
123  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
124  }
125  if (!m_devices_count) {
126  throw(vpException(vpException::fatalError, "No Kinova robot found"));
127  }
128 
129  (void)eJe;
130  std::cout << "Not implemented ! " << std::endl;
131 }
132 
141 {
142  if (!m_plugin_loaded) {
143  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
144  }
145  if (!m_devices_count) {
146  throw(vpException(vpException::fatalError, "No Kinova robot found"));
147  }
148 
149  (void)fJe;
150  std::cout << "Not implemented ! " << std::endl;
151 }
152 
153 /*
154 
155  At least one of these function has to be implemented to control the robot:
156  - setCartVelocity()
157  - setJointVelocity()
158 
159 */
160 
176 {
177  if (v.size() != 6) {
179  "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
180  }
181 
182  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
183  vpColVector v_c; // This is the velocity that the robot is able to apply in the camera frame
184  vpColVector v_mix; // This is the velocity that the robot is able to apply in the mix frame
185  switch (frame) {
186  case vpRobot::CAMERA_FRAME: {
187  // We have to transform the requested velocity in the end-effector frame.
188  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
189  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
190  // a velocity twist from tool (or camera) frame into end-effector frame
191  vpVelocityTwistMatrix eVc(m_eMc); // GET IT FROM CAMERA EXTRINSIC CALIBRATION FILE
192 
193  // Input velocity is expressed in camera or tool frame
194  v_c = v;
195 
196  // Tranform velocity in end-effector
197  v_e = eVc * v_c;
198 
199  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
200  vpColVector p_e;
202  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
203  vpRotationMatrix bRe(bre);
204  vpMatrix bVe(6, 6, 0);
205  bVe.eye();
206  bVe.insert(bRe, 0, 0);
207  v_mix = bVe * v_e;
208 
209  TrajectoryPoint pointToSend;
210  pointToSend.InitStruct();
211  // We specify that this point will be used an angular (joint by joint) velocity vector
212  pointToSend.Position.Type = CARTESIAN_VELOCITY;
213  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
214 
215  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
216  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
217  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
218  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
219  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
220  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
221 
222  KinovaSetCartesianControl(); // Not sure that this function is useful here
223 
224  KinovaSendBasicTrajectory(pointToSend);
225  break;
226  }
227 
229  // Input velocity is expressed in end-effector
230  v_e = v;
231 
232  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
233  vpColVector p_e;
235  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
236  vpRotationMatrix bRe(bre);
237  vpMatrix bVe(6, 6, 0);
238  bVe.eye();
239  bVe.insert(bRe, 0, 0);
240  v_mix = bVe * v_e;
241 
242  TrajectoryPoint pointToSend;
243  pointToSend.InitStruct();
244  // We specify that this point will be used an angular (joint by joint) velocity vector
245  pointToSend.Position.Type = CARTESIAN_VELOCITY;
246  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
247 
248  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
249  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
250  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
251  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
252  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
253  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
254 
255  KinovaSetCartesianControl(); // Not sure that this function is useful here
256 
257  KinovaSendBasicTrajectory(pointToSend);
258  break;
259  }
260 
261  case vpRobot::MIXT_FRAME: {
262 
263  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
264  vpColVector p_e;
266  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
267  vpRotationMatrix bRe(bre);
268  std::cout << "rotation matrix from base to endeffector is bRe : " << std::endl;
269  std::cout << "bRe:\n" << bRe << std::endl;
270  vpMatrix bVe(6, 6, 0);
271  bVe.eye();
272  bVe.insert(bRe, 0, 0);
273  v_e = v;
274  // vpColVector bVe;
275  vpColVector v_mix = bVe * v_e;
276 
277  TrajectoryPoint pointToSend;
278  pointToSend.InitStruct();
279  // We specify that this point will be used an angular (joint by joint) velocity vector
280  pointToSend.Position.Type = CARTESIAN_VELOCITY;
281  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
282 
283  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
284  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
285  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
286  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_e[3]);
287  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_e[4]);
288  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_e[5]);
289 
290  KinovaSetCartesianControl(); // Not sure that this function is useful here
291  KinovaSendBasicTrajectory(pointToSend);
292  break;
293  }
296  // Out of the scope
297  break;
298  }
299 }
300 
306 {
307  if (qdot.size() != static_cast<unsigned int>(nDof)) {
309  "Cannot apply a %d-dim joint velocity vector to the Jaco robot configured with %d DoF",
310  qdot.size(), nDof));
311  }
312  TrajectoryPoint pointToSend;
313  pointToSend.InitStruct();
314  // We specify that this point will be used an angular (joint by joint) velocity vector
315  pointToSend.Position.Type = ANGULAR_VELOCITY;
316  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
317  switch (nDof) {
318  case 7: {
319  pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(qdot[6]));
320  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
321  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
322  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
323  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
324  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
325  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
326  break;
327  }
328  case 6: {
329  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(qdot[5]));
330  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(qdot[4]));
331  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
332  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
333  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
334  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
335  break;
336  }
337  case 4: {
338  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(qdot[3]));
339  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(qdot[2]));
340  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(qdot[1]));
341  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(qdot[0]));
342  break;
343  }
344  default:
345  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
346  }
347 
348  KinovaSetAngularControl(); // Not sure that this function is useful here
349 
350  KinovaSendBasicTrajectory(pointToSend);
351 }
352 
371 {
372  if (!m_plugin_loaded) {
373  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
374  }
375  if (!m_devices_count) {
376  throw(vpException(vpException::fatalError, "No Kinova robot found"));
377  }
380  "Cannot send a velocity to the robot. "
381  "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
382  "entering your control loop.");
383  }
384 
385  vpColVector vel_sat(6);
386 
387  // Velocity saturation
388  switch (frame) {
389  // Saturation in cartesian space
390  case vpRobot::TOOL_FRAME:
393  case vpRobot::MIXT_FRAME: {
394  if (vel.size() != 6) {
396  "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
397  }
398  vpColVector vel_max(6);
399 
400  for (unsigned int i = 0; i < 3; i++)
401  vel_max[i] = getMaxTranslationVelocity();
402  for (unsigned int i = 3; i < 6; i++)
403  vel_max[i] = getMaxRotationVelocity();
404 
405  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
406 
407  setCartVelocity(frame, vel_sat);
408  break;
409  }
410 
411  // Saturation in joint space
412  case vpRobot::JOINT_STATE: {
413  if (vel.size() != static_cast<size_t>(nDof)) {
414  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)",
415  nDof, vel.size()));
416  }
417  vpColVector vel_max(vel.size());
418 
419  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
420  vel_max = getMaxRotationVelocity();
421 
422  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
423 
424  setJointVelocity(vel_sat);
425  }
426  }
427 }
428 
429 /*
430 
431  THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
432 
433 */
434 
443 {
444  AngularPosition currentCommand;
445 
446  // We get the actual angular command of the robot. Values are in deg
447  KinovaGetAngularCommand(currentCommand);
448 
449  q.resize(nDof);
450  switch (nDof) {
451  case 7: {
452  q[6] = vpMath::rad(currentCommand.Actuators.Actuator7);
453  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
454  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
455  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
456  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
457  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
458  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
459  break;
460  }
461  case 6: {
462  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
463  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
464  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
465  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
466  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
467  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
468  break;
469  }
470  case 4: {
471  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
472  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
473  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
474  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
475  break;
476  }
477  default:
478  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
479  }
480 }
481 
511 {
512  if (!m_plugin_loaded) {
513  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
514  }
515  if (!m_devices_count) {
516  throw(vpException(vpException::fatalError, "No Kinova robot found"));
517  }
518 
519  if (frame == JOINT_STATE) {
520  getJointPosition(position);
521  } else if (frame == END_EFFECTOR_FRAME) {
522  CartesianPosition currentCommand;
523  // We get the actual cartesian position of the robot
524  KinovaGetCartesianCommand(currentCommand);
525  position.resize(6);
526  position[0] = currentCommand.Coordinates.X;
527  position[1] = currentCommand.Coordinates.Y;
528  position[2] = currentCommand.Coordinates.Z;
529  position[3] = currentCommand.Coordinates.ThetaX;
530  position[4] = currentCommand.Coordinates.ThetaY;
531  position[5] = currentCommand.Coordinates.ThetaZ;
532  } else {
533  std::cout << "Not implemented ! " << std::endl;
534  }
535 }
536 
557 {
558  if (frame == JOINT_STATE) {
559  throw(vpException(vpException::fatalError, "Cannot get Jaco joint position as a pose vector"));
560  }
561 
562  vpColVector position;
563  getPosition(frame, position);
564 
565  vpRxyzVector rxyz; // reference frame to end-effector frame rotations
566  // Update the transformation between reference frame and end-effector frame
567  for (unsigned int i = 0; i < 3; i++) {
568  pose[i] = position[i]; // tx, ty, tz
569  rxyz[i] = position[i + 3]; // ry, ry, rz
570  }
571  vpThetaUVector tu(rxyz);
572  for (unsigned int i = 0; i < 3; i++) {
573  pose[i + 3] = tu[i]; // tux, tuy, tuz
574  }
575 }
576 
584 {
585  if (!m_plugin_loaded) {
586  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
587  }
588  if (!m_devices_count) {
589  throw(vpException(vpException::fatalError, "No Kinova robot found"));
590  }
591  if (frame == vpRobot::JOINT_STATE) {
592  if (static_cast<int>(q.size()) != nDof) {
594  "Cannot move robot to a joint position of dim %u that is not a %d-dim vector", q.size(), nDof));
595  }
596  TrajectoryPoint pointToSend;
597  pointToSend.InitStruct();
598  // We specify that this point will be an angular(joint by joint) position.
599  pointToSend.Position.Type = ANGULAR_POSITION;
600  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
601  switch (nDof) {
602  case 7: {
603  pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(q[6]));
604  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
605  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
606  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
607  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
608  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
609  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
610  break;
611  }
612  case 6: {
613  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
614  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
615  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
616  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
617  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
618  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
619  break;
620  }
621  case 4: {
622  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
623  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
624  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
625  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
626  break;
627  }
628  default:
629  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
630  }
631 
632  KinovaSetAngularControl(); // Not sure that this function is useful here
633 
634  if (m_verbose) {
635  std::cout << "Move robot to joint position [rad rad rad rad rad rad]: " << q.t() << std::endl;
636  }
637  KinovaSendBasicTrajectory(pointToSend);
638  } else if (frame == vpRobot::END_EFFECTOR_FRAME) {
639  if (q.size() != 6) {
641  "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.size()));
642  }
643  TrajectoryPoint pointToSend;
644  pointToSend.InitStruct();
645  // We specify that this point will be an angular(joint by joint) position.
646  pointToSend.Position.Type = CARTESIAN_POSITION;
647  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
648  pointToSend.Position.CartesianPosition.X = static_cast<float>(q[0]);
649  pointToSend.Position.CartesianPosition.Y = static_cast<float>(q[1]);
650  pointToSend.Position.CartesianPosition.Z = static_cast<float>(q[2]);
651  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(q[3]);
652  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(q[4]);
653  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(q[5]);
654 
655  KinovaSetCartesianControl(); // Not sure that this function is useful here
656 
657  if (m_verbose) {
658  std::cout << "Move robot to cartesian position [m m m rad rad rad]: " << q.t() << std::endl;
659  }
660  KinovaSendBasicTrajectory(pointToSend);
661 
662  } else {
664  "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
665  }
666 }
667 
675 {
676  if (!m_plugin_loaded) {
677  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
678  }
679  if (!m_devices_count) {
680  throw(vpException(vpException::fatalError, "No Kinova robot found"));
681  }
682 
683  (void)frame;
684  (void)q;
685  std::cout << "Not implemented ! " << std::endl;
686 }
687 
700 {
702  throw(vpException(vpException::fatalError, "Kinova robot command layer unset"));
703  }
704  if (m_plugin_loaded) {
705  closePlugin();
706  }
707 #ifdef __linux__
708  // We load the API
709  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("Kinova.API.USBCommandLayerUbuntu.so")
710  : std::string("Kinova.API.EthCommandLayerUbuntu.so");
711  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
712  if (m_verbose) {
713  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
714  }
715  m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
716 
717  std::string prefix = (m_command_layer == CMD_LAYER_USB) ? std::string("") : std::string("Ethernet_");
718  // We load the functions from the library
719  KinovaCloseAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("CloseAPI")).c_str());
720  KinovaGetAngularCommand =
721  (int (*)(AngularPosition &))dlsym(m_command_layer_handle, (prefix + std::string("GetAngularCommand")).c_str());
722  KinovaGetCartesianCommand = (int (*)(CartesianPosition &))dlsym(
723  m_command_layer_handle, (prefix + std::string("GetCartesianCommand")).c_str());
724  KinovaGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result))dlsym(
725  m_command_layer_handle, (prefix + std::string("GetDevices")).c_str());
726  KinovaInitAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitAPI")).c_str());
727  KinovaInitFingers = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("InitFingers")).c_str());
728  KinovaMoveHome = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("MoveHome")).c_str());
729  KinovaSendBasicTrajectory =
730  (int (*)(TrajectoryPoint))dlsym(m_command_layer_handle, (prefix + std::string("SendBasicTrajectory")).c_str());
731  KinovaSetActiveDevice =
732  (int (*)(KinovaDevice devices))dlsym(m_command_layer_handle, (prefix + std::string("SetActiveDevice")).c_str());
733  KinovaSetAngularControl =
734  (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetAngularControl")).c_str());
735  KinovaSetCartesianControl =
736  (int (*)())dlsym(m_command_layer_handle, (prefix + std::string("SetCartesianControl")).c_str());
737 #elif _WIN32
738  // We load the API.
739  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("CommandLayerWindows.dll")
740  : std::string("CommandLayerEthernet.dll");
741  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
742  if (m_verbose) {
743  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
744  }
745  m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
746 
747  // We load the functions from the library
748  KinovaCloseAPI = (int (*)())GetProcAddress(m_command_layer_handle, "CloseAPI");
749  KinovaGetAngularCommand = (int (*)(AngularPosition &))GetProcAddress(m_command_layer_handle, "GetAngularCommand");
750  KinovaGetCartesianCommand =
751  (int (*)(CartesianPosition &))GetProcAddress(m_command_layer_handle, "GetCartesianCommand");
752  KinovaGetDevices =
753  (int (*)(KinovaDevice[MAX_KINOVA_DEVICE], int &))GetProcAddress(m_command_layer_handle, "GetDevices");
754  KinovaInitAPI = (int (*)())GetProcAddress(m_command_layer_handle, "InitAPI");
755  KinovaInitFingers = (int (*)())GetProcAddress(m_command_layer_handle, "InitFingers");
756  KinovaMoveHome = (int (*)())GetProcAddress(m_command_layer_handle, "MoveHome");
757  KinovaSendBasicTrajectory = (int (*)(TrajectoryPoint))GetProcAddress(m_command_layer_handle, "SendBasicTrajectory");
758  KinovaSetActiveDevice = (int (*)(KinovaDevice))GetProcAddress(m_command_layer_handle, "SetActiveDevice");
759  KinovaSetAngularControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetAngularControl");
760  KinovaSetCartesianControl = (int (*)())GetProcAddress(m_command_layer_handle, "SetCartesianControl");
761 #endif
762 
763  // Verify that all functions has been loaded correctly
764  if ((KinovaCloseAPI == nullptr) || (KinovaGetAngularCommand == nullptr) || (KinovaGetAngularCommand == nullptr) ||
765  (KinovaGetCartesianCommand == nullptr) || (KinovaGetDevices == nullptr) || (KinovaInitAPI == nullptr) ||
766  (KinovaInitFingers == nullptr) || (KinovaMoveHome == nullptr) || (KinovaSendBasicTrajectory == nullptr) ||
767  (KinovaSetActiveDevice == nullptr) || (KinovaSetAngularControl == nullptr) || (KinovaSetCartesianControl == nullptr)) {
768  throw(vpException(vpException::fatalError, "Cannot load plugin from \"%s\" folder", m_plugin_location.c_str()));
769  }
770  if (m_verbose) {
771  std::cout << "Plugin successfully loaded" << std::endl;
772  }
773 
774  m_plugin_loaded = true;
775 }
776 
781 {
782  if (m_plugin_loaded) {
783  if (m_verbose) {
784  std::cout << "Close plugin" << std::endl;
785  }
786 #ifdef __linux__
787  dlclose(m_command_layer_handle);
788 #elif _WIN32
789  FreeLibrary(m_command_layer_handle);
790 #endif
791  m_plugin_loaded = false;
792  }
793 }
794 
799 {
800  if (!m_plugin_loaded) {
801  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
802  }
803  if (!m_devices_count) {
804  throw(vpException(vpException::fatalError, "No Kinova robot found"));
805  }
806 
807  if (m_verbose) {
808  std::cout << "Move the robot to home position" << std::endl;
809  }
810  KinovaMoveHome();
811 }
812 
818 {
819  loadPlugin();
820  if (!m_plugin_loaded) {
821  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
822  }
823 
824  int result = (*KinovaInitAPI)();
825 
826  if (m_verbose) {
827  std::cout << "Initialization's result: " << result << std::endl;
828  }
829 
830  m_devices_count = KinovaGetDevices(m_devices_list, result);
831 
832  if (m_verbose) {
833  std::cout << "Found " << m_devices_count << " devices" << std::endl;
834  }
835 
836  // By default set the first device as active
837  setActiveDevice(0);
838 
839  // Initialize fingers
840  KinovaInitFingers();
841 
842  return m_devices_count;
843 }
844 
856 {
857  if (!m_plugin_loaded) {
858  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
859  }
860  if (!m_devices_count) {
861  throw(vpException(vpException::fatalError, "No Kinova robot found"));
862  }
863  if (device < 0 || device >= m_devices_count) {
864  throw(vpException(vpException::badValue, "Cannot set Kinova active device %d. Value should be in range [0, %d]",
865  device, m_devices_count - 1));
866  }
867  if (device != m_active_device) {
868  m_active_device = device;
869  KinovaSetActiveDevice(m_devices_list[m_active_device]);
870  }
871 }
872 
873 #elif !defined(VISP_BUILD_SHARED_LIBS)
874 // Work around to avoid warning: libvisp_robot.a(vpRobotKinova.cpp.o) has
875 // no symbols
876 void dummy_vpRobotKinova(){};
877 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:70
@ fatalError
Fatal error.
Definition: vpException.h:71
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:2197
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:448
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5781
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:190
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
KinovaDevice * m_devices_list
void setDoF(unsigned int dof)
std::string m_plugin_location
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
CommandLayer m_command_layer
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
void get_fJe(vpMatrix &fJe) vp_override
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
void setJointVelocity(const vpColVector &qdot)
void getJointPosition(vpColVector &q)
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setActiveDevice(int device)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
void get_eJe(vpMatrix &eJe) vp_override
virtual ~vpRobotKinova() vp_override
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ TOOL_FRAME
Definition: vpRobot.h:84
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
double maxRotationVelocity
Definition: vpRobot.h:98
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:177
Implementation of a rotation vector as axis-angle minimal representation.