Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  }
85  else {
86  throw(vpException(vpException::fatalError, "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) {
181  throw(vpException(vpException::fatalError, "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.size()));
182  }
183 
184  vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
185  vpColVector v_c; // This is the velocity that the robot is able to apply in the camera frame
186  vpColVector v_mix; // This is the velocity that the robot is able to apply in the mix frame
187  switch (frame) {
188  case vpRobot::CAMERA_FRAME: {
189  // We have to transform the requested velocity in the end-effector frame.
190  // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
191  // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
192  // a velocity twist from tool (or camera) frame into end-effector frame
193  vpVelocityTwistMatrix eVc(m_eMc); // GET IT FROM CAMERA EXTRINSIC CALIBRATION FILE
194 
195  // Input velocity is expressed in camera or tool frame
196  v_c = v;
197 
198  // Tranform velocity in end-effector
199  v_e = eVc * v_c;
200 
201  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
202  vpColVector p_e;
204  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
205  vpRotationMatrix bRe(bre);
206  vpMatrix bVe(6, 6, 0);
207  bVe.eye();
208  bVe.insert(bRe, 0, 0);
209  v_mix = bVe * v_e;
210 
211  TrajectoryPoint pointToSend;
212  pointToSend.InitStruct();
213  // We specify that this point will be used an angular (joint by joint) velocity vector
214  pointToSend.Position.Type = CARTESIAN_VELOCITY;
215  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
216 
217  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
218  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
219  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
220  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
221  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
222  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
223 
224  KinovaSetCartesianControl(); // Not sure that this function is useful here
225 
226  KinovaSendBasicTrajectory(pointToSend);
227  break;
228  }
229 
231  // Input velocity is expressed in end-effector
232  v_e = v;
233 
234  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
235  vpColVector p_e;
237  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
238  vpRotationMatrix bRe(bre);
239  vpMatrix bVe(6, 6, 0);
240  bVe.eye();
241  bVe.insert(bRe, 0, 0);
242  v_mix = bVe * v_e;
243 
244  TrajectoryPoint pointToSend;
245  pointToSend.InitStruct();
246  // We specify that this point will be used an angular (joint by joint) velocity vector
247  pointToSend.Position.Type = CARTESIAN_VELOCITY;
248  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
249 
250  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
251  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
252  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
253  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_mix[3]);
254  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_mix[4]);
255  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_mix[5]);
256 
257  KinovaSetCartesianControl(); // Not sure that this function is useful here
258 
259  KinovaSendBasicTrajectory(pointToSend);
260  break;
261  }
262 
263  case vpRobot::MIXT_FRAME: {
264 
265  // Convert end-effector translation velocity in base frame, rotation velocity is unchanged
266  vpColVector p_e;
268  vpRxyzVector bre(p_e[3], p_e[4], p_e[5]);
269  vpRotationMatrix bRe(bre);
270  std::cout << "rotation matrix from base to endeffector is bRe : " << std::endl;
271  std::cout << "bRe:\n" << bRe << std::endl;
272  vpMatrix bVe(6, 6, 0);
273  bVe.eye();
274  bVe.insert(bRe, 0, 0);
275  v_e = v;
276  //vpColVector bVe;
277  vpColVector v_mix = bVe * v_e;
278 
279  TrajectoryPoint pointToSend;
280  pointToSend.InitStruct();
281  // We specify that this point will be used an angular (joint by joint) velocity vector
282  pointToSend.Position.Type = CARTESIAN_VELOCITY;
283  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
284 
285  pointToSend.Position.CartesianPosition.X = static_cast<float>(v_mix[0]);
286  pointToSend.Position.CartesianPosition.Y = static_cast<float>(v_mix[1]);
287  pointToSend.Position.CartesianPosition.Z = static_cast<float>(v_mix[2]);
288  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(v_e[3]);
289  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(v_e[4]);
290  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(v_e[5]);
291 
292  KinovaSetCartesianControl(); // Not sure that this function is useful here
293  KinovaSendBasicTrajectory(pointToSend);
294  break;
295  }
298  // Out of the scope
299  break;
300  }
301 }
302 
308 {
309  if (qdot.size() != static_cast<unsigned int>(nDof)) {
310  throw(vpException(vpException::dimensionError, "Cannot apply a %d-dim joint velocity vector to the Jaco robot configured with %d DoF", 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) {
395  throw(vpException(vpException::dimensionError, "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
396  }
397  vpColVector vel_max(6);
398 
399  for (unsigned int i = 0; i < 3; i++)
400  vel_max[i] = getMaxTranslationVelocity();
401  for (unsigned int i = 3; i < 6; i++)
402  vel_max[i] = getMaxRotationVelocity();
403 
404  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
405 
406  setCartVelocity(frame, vel_sat);
407  break;
408  }
409 
410  // Saturation in joint space
411  case vpRobot::JOINT_STATE: {
412  if (vel.size() != static_cast<size_t>(nDof)) {
413  throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %-dim vector (%d)", nDof, vel.size()));
414  }
415  vpColVector vel_max(vel.size());
416 
417  // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
418  vel_max = getMaxRotationVelocity();
419 
420  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
421 
422  setJointVelocity(vel_sat);
423  }
424  }
425 }
426 
427 /*
428 
429  THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
430 
431 */
432 
441 {
442  AngularPosition currentCommand;
443 
444  // We get the actual angular command of the robot. Values are in deg
445  KinovaGetAngularCommand(currentCommand);
446 
447  q.resize(nDof);
448  switch (nDof) {
449  case 7: {
450  q[6] = vpMath::rad(currentCommand.Actuators.Actuator7);
451  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
452  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
453  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
454  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
455  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
456  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
457  break;
458  }
459  case 6: {
460  q[5] = vpMath::rad(currentCommand.Actuators.Actuator6);
461  q[4] = vpMath::rad(currentCommand.Actuators.Actuator5);
462  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
463  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
464  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
465  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
466  break;
467  }
468  case 4: {
469  q[3] = vpMath::rad(currentCommand.Actuators.Actuator4);
470  q[2] = vpMath::rad(currentCommand.Actuators.Actuator3);
471  q[1] = vpMath::rad(currentCommand.Actuators.Actuator2);
472  q[0] = vpMath::rad(currentCommand.Actuators.Actuator1);
473  break;
474  }
475  default:
476  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
477  }
478 }
479 
508 {
509  if (!m_plugin_loaded) {
510  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
511  }
512  if (!m_devices_count) {
513  throw(vpException(vpException::fatalError, "No Kinova robot found"));
514  }
515 
516  if (frame == JOINT_STATE) {
517  getJointPosition(position);
518  }
519  else if (frame == END_EFFECTOR_FRAME) {
520  CartesianPosition currentCommand;
521  // We get the actual cartesian position of the robot
522  KinovaGetCartesianCommand(currentCommand);
523  position.resize(6);
524  position[0] = currentCommand.Coordinates.X;
525  position[1] = currentCommand.Coordinates.Y;
526  position[2] = currentCommand.Coordinates.Z;
527  position[3] = currentCommand.Coordinates.ThetaX;
528  position[4] = currentCommand.Coordinates.ThetaY;
529  position[5] = currentCommand.Coordinates.ThetaZ;
530  }
531  else {
532  std::cout << "Not implemented ! " << std::endl;
533  }
534 }
535 
555 {
556  if (frame == JOINT_STATE) {
557  throw(vpException(vpException::fatalError, "Cannot get Jaco joint position as a pose vector"));
558  }
559 
560  vpColVector position;
561  getPosition(frame, position);
562 
563  vpRxyzVector rxyz; // reference frame to end-effector frame rotations
564  // Update the transformation between reference frame and end-effector frame
565  for (unsigned int i=0; i < 3; i++) {
566  pose[i] = position[i]; // tx, ty, tz
567  rxyz[i] = position[i+3]; // ry, ry, rz
568  }
569  vpThetaUVector tu(rxyz);
570  for (unsigned int i=0; i < 3; i++) {
571  pose[i+3] = tu[i]; // tux, tuy, tuz
572  }
573 }
574 
582 {
583  if (!m_plugin_loaded) {
584  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
585  }
586  if (!m_devices_count) {
587  throw(vpException(vpException::fatalError, "No Kinova robot found"));
588  }
589  if (frame == vpRobot::JOINT_STATE) {
590  if (static_cast<int>(q.size()) != nDof) {
591  throw(vpException(vpException::fatalError, "Cannot move robot to a joint position of dim %u that is not a %d-dim vector", q.size(), nDof));
592  }
593  TrajectoryPoint pointToSend;
594  pointToSend.InitStruct();
595  // We specify that this point will be an angular(joint by joint) position.
596  pointToSend.Position.Type = ANGULAR_POSITION;
597  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
598  switch (nDof) {
599  case 7: {
600  pointToSend.Position.Actuators.Actuator7 = static_cast<float>(vpMath::deg(q[6]));
601  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
602  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
603  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
604  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
605  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
606  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
607  break;
608  }
609  case 6: {
610  pointToSend.Position.Actuators.Actuator6 = static_cast<float>(vpMath::deg(q[5]));
611  pointToSend.Position.Actuators.Actuator5 = static_cast<float>(vpMath::deg(q[4]));
612  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
613  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
614  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
615  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
616  break;
617  }
618  case 4: {
619  pointToSend.Position.Actuators.Actuator4 = static_cast<float>(vpMath::deg(q[3]));
620  pointToSend.Position.Actuators.Actuator3 = static_cast<float>(vpMath::deg(q[2]));
621  pointToSend.Position.Actuators.Actuator2 = static_cast<float>(vpMath::deg(q[1]));
622  pointToSend.Position.Actuators.Actuator1 = static_cast<float>(vpMath::deg(q[0]));
623  break;
624  }
625  default:
626  throw(vpException(vpException::fatalError, "Jaco robot non supported %d DoF", nDof));
627  }
628 
629  KinovaSetAngularControl(); // Not sure that this function is useful here
630 
631  if (m_verbose) {
632  std::cout << "Move robot to joint position [rad rad rad rad rad rad]: " << q.t() << std::endl;
633  }
634  KinovaSendBasicTrajectory(pointToSend);
635  }
636  else if (frame == vpRobot::END_EFFECTOR_FRAME) {
637  if (q.size() != 6) {
638  throw(vpException(vpException::fatalError, "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.size()));
639  }
640  TrajectoryPoint pointToSend;
641  pointToSend.InitStruct();
642  // We specify that this point will be an angular(joint by joint) position.
643  pointToSend.Position.Type = CARTESIAN_POSITION;
644  pointToSend.Position.HandMode = HAND_NOMOVEMENT;
645  pointToSend.Position.CartesianPosition.X = static_cast<float>(q[0]);
646  pointToSend.Position.CartesianPosition.Y = static_cast<float>(q[1]);
647  pointToSend.Position.CartesianPosition.Z = static_cast<float>(q[2]);
648  pointToSend.Position.CartesianPosition.ThetaX = static_cast<float>(q[3]);
649  pointToSend.Position.CartesianPosition.ThetaY = static_cast<float>(q[4]);
650  pointToSend.Position.CartesianPosition.ThetaZ = static_cast<float>(q[5]);
651 
652  KinovaSetCartesianControl(); // Not sure that this function is useful here
653 
654  if (m_verbose) {
655  std::cout << "Move robot to cartesian position [m m m rad rad rad]: " << q.t() << std::endl;
656  }
657  KinovaSendBasicTrajectory(pointToSend);
658 
659  }
660  else {
661  throw(vpException(vpException::fatalError, "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
662  }
663 }
664 
672 {
673  if (!m_plugin_loaded) {
674  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
675  }
676  if (!m_devices_count) {
677  throw(vpException(vpException::fatalError, "No Kinova robot found"));
678  }
679 
680  (void)frame;
681  (void)q;
682  std::cout << "Not implemented ! " << std::endl;
683 }
684 
697 {
699  throw(vpException(vpException::fatalError, "Kinova robot command layer unset"));
700  }
701  if (m_plugin_loaded) {
702  closePlugin();
703  }
704 #ifdef __linux__
705  // We load the API
706  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("Kinova.API.USBCommandLayerUbuntu.so") : std::string("Kinova.API.EthCommandLayerUbuntu.so");
707  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
708  if (m_verbose) {
709  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
710  }
711  m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
712 
713  std::string prefix = (m_command_layer == CMD_LAYER_USB) ? std::string("") : std::string("Ethernet_");
714  // We load the functions from the library
715  KinovaCloseAPI = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("CloseAPI")).c_str());
716  KinovaGetAngularCommand = (int(*)(AngularPosition &)) dlsym(m_command_layer_handle, (prefix + std::string("GetAngularCommand")).c_str());
717  KinovaGetCartesianCommand = (int(*)(CartesianPosition &)) dlsym(m_command_layer_handle, (prefix + std::string("GetCartesianCommand")).c_str());
718  KinovaGetDevices = (int(*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) dlsym(m_command_layer_handle, (prefix + std::string("GetDevices")).c_str());
719  KinovaInitAPI = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("InitAPI")).c_str());
720  KinovaInitFingers = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("InitFingers")).c_str());
721  KinovaMoveHome = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("MoveHome")).c_str());
722  KinovaSendBasicTrajectory = (int(*)(TrajectoryPoint)) dlsym(m_command_layer_handle, (prefix + std::string("SendBasicTrajectory")).c_str());
723  KinovaSetActiveDevice = (int(*)(KinovaDevice devices)) dlsym(m_command_layer_handle, (prefix + std::string("SetActiveDevice")).c_str());
724  KinovaSetAngularControl = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("SetAngularControl")).c_str());
725  KinovaSetCartesianControl = (int(*)()) dlsym(m_command_layer_handle, (prefix + std::string("SetCartesianControl")).c_str());
726 #elif _WIN32
727  // We load the API.
728  std::string plugin_name = (m_command_layer == CMD_LAYER_USB) ? std::string("CommandLayerWindows.dll") : std::string("CommandLayerEthernet.dll");
729  std::string plugin = vpIoTools::createFilePath(m_plugin_location, plugin_name);
730  if (m_verbose) {
731  std::cout << "Load plugin: \"" << plugin << "\"" << std::endl;
732  }
733  m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
734 
735  // We load the functions from the library
736  KinovaCloseAPI = (int(*)()) GetProcAddress(m_command_layer_handle, "CloseAPI");
737  KinovaGetAngularCommand = (int(*)(AngularPosition &)) GetProcAddress(m_command_layer_handle, "GetAngularCommand");
738  KinovaGetCartesianCommand = (int(*)(CartesianPosition &)) GetProcAddress(m_command_layer_handle, "GetCartesianCommand");
739  KinovaGetDevices = (int(*)(KinovaDevice[MAX_KINOVA_DEVICE], int&)) GetProcAddress(m_command_layer_handle, "GetDevices");
740  KinovaInitAPI = (int(*)()) GetProcAddress(m_command_layer_handle, "InitAPI");
741  KinovaInitFingers = (int(*)()) GetProcAddress(m_command_layer_handle, "InitFingers");
742  KinovaMoveHome = (int(*)()) GetProcAddress(m_command_layer_handle, "MoveHome");
743  KinovaSendBasicTrajectory = (int(*)(TrajectoryPoint)) GetProcAddress(m_command_layer_handle, "SendBasicTrajectory");
744  KinovaSetActiveDevice = (int(*)(KinovaDevice)) GetProcAddress(m_command_layer_handle, "SetActiveDevice");
745  KinovaSetAngularControl = (int(*)()) GetProcAddress(m_command_layer_handle, "SetAngularControl");
746  KinovaSetCartesianControl = (int(*)()) GetProcAddress(m_command_layer_handle, "SetCartesianControl");
747 #endif
748 
749  // Verify that all functions has been loaded correctly
750  if ((KinovaCloseAPI == NULL) ||
751  (KinovaGetAngularCommand == NULL) || (KinovaGetAngularCommand == NULL) || (KinovaGetCartesianCommand == NULL) || (KinovaGetDevices == NULL) ||
752  (KinovaInitAPI == NULL) || (KinovaInitFingers == NULL) ||
753  (KinovaMoveHome == NULL) || (KinovaSendBasicTrajectory == NULL) ||
754  (KinovaSetActiveDevice == NULL) || (KinovaSetAngularControl == NULL) || (KinovaSetCartesianControl == NULL)) {
755  throw(vpException(vpException::fatalError, "Cannot load plugin from \"%s\" folder", m_plugin_location.c_str()));
756  }
757  if (m_verbose) {
758  std::cout << "Plugin successfully loaded" << std::endl;
759  }
760 
761  m_plugin_loaded = true;
762 }
763 
768 {
769  if (m_plugin_loaded) {
770  if (m_verbose) {
771  std::cout << "Close plugin" << std::endl;
772  }
773 #ifdef __linux__
774  dlclose(m_command_layer_handle);
775 #elif _WIN32
776  FreeLibrary(m_command_layer_handle);
777 #endif
778  m_plugin_loaded = false;
779  }
780 }
781 
786 {
787  if (!m_plugin_loaded) {
788  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
789  }
790  if (!m_devices_count) {
791  throw(vpException(vpException::fatalError, "No Kinova robot found"));
792  }
793 
794  if (m_verbose) {
795  std::cout << "Move the robot to home position" << std::endl;
796  }
797  KinovaMoveHome();
798 }
799 
805 {
806  loadPlugin();
807  if (!m_plugin_loaded) {
808  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
809  }
810 
811  int result = (*KinovaInitAPI)();
812 
813  if (m_verbose) {
814  std::cout << "Initialization's result: " << result << std::endl;
815  }
816 
817  m_devices_count = KinovaGetDevices(m_devices_list, result);
818 
819  if (m_verbose) {
820  std::cout << "Found " << m_devices_count << " devices" << std::endl;
821  }
822 
823  // By default set the first device as active
824  setActiveDevice(0);
825 
826  // Initialize fingers
827  KinovaInitFingers();
828 
829  return m_devices_count;
830 }
831 
843 {
844  if (!m_plugin_loaded) {
845  throw(vpException(vpException::fatalError, "Jaco SDK plugin not loaded"));
846  }
847  if (!m_devices_count) {
848  throw(vpException(vpException::fatalError, "No Kinova robot found"));
849  }
850  if (device < 0 || device >= m_devices_count) {
851  throw(vpException(vpException::badValue, "Cannot set Kinova active device %d. Value should be in range [0, %d]", device, m_devices_count - 1));
852  }
853  if (device != m_active_device) {
854  m_active_device = device;
855  KinovaSetActiveDevice(m_devices_list[m_active_device]);
856  }
857 }
858 
859 #elif !defined(VISP_BUILD_SHARED_LIBS)
860  // Work arround to avoid warning: libvisp_robot.a(vpRobotKinova.cpp.o) has
861  // no symbols
862 void dummy_vpRobotKinova() {};
863 #endif
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Error that can be emited by the vpRobot class and its derivates.
KinovaDevice * m_devices_list
double maxTranslationVelocity
Definition: vpRobot.h:96
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:98
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpRowVector t() const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void setActiveDevice(int device)
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
void setJointVelocity(const vpColVector &qdot)
virtual ~vpRobotKinova()
static const double maxTranslationVelocityDefault
Definition: vpRobot.h:97
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
Initialize the velocity controller.
Definition: vpRobot.h:66
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
std::string m_plugin_location
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static const double maxRotationVelocityDefault
Definition: vpRobot.h:99
CommandLayer m_command_layer
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static double rad(double deg)
Definition: vpMath.h:110
void setDoF(unsigned int dof)
void get_fJe(vpMatrix &fJe)
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static double deg(double rad)
Definition: vpMath.h:103
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void get_eJe(vpMatrix &eJe)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
void eye()
Definition: vpMatrix.cpp:449
Implementation of a rotation vector as axis-angle minimal representation.
void getJointPosition(vpColVector &q)