Visual Servoing Platform version 3.5.0
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{
70
71 if (m_devices_list) {
72 delete[] m_devices_list;
73 }
74}
75
80void 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) {
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
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
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
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
862void dummy_vpRobotKinova() {};
863#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:1670
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:449
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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: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:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
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:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
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:251
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:184
Implementation of a rotation vector as axis-angle minimal representation.