Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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(NULL), m_active_device(-1), m_command_layer(CMD_LAYER_UNSET), m_command_layer_handle()
57{
58 init();
59}
60
65{
67
68 if (m_devices_list) {
69 delete[] m_devices_list;
70 }
71}
72
77void 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) {
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
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
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 == NULL) || (KinovaGetAngularCommand == NULL) || (KinovaGetAngularCommand == NULL) ||
765 (KinovaGetCartesianCommand == NULL) || (KinovaGetDevices == NULL) || (KinovaInitAPI == NULL) ||
766 (KinovaInitFingers == NULL) || (KinovaMoveHome == NULL) || (KinovaSendBasicTrajectory == NULL) ||
767 (KinovaSetActiveDevice == NULL) || (KinovaSetAngularControl == NULL) || (KinovaSetCartesianControl == NULL)) {
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
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
876void dummy_vpRobotKinova(){};
877#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
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:85
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
static std::string createFilePath(const std::string &parent, const std::string &child)
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:152
void eye()
Definition vpMatrix.cpp:446
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
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:100
static const double maxTranslationVelocityDefault
Definition vpRobot.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition vpRobot.h:102
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ JOINT_STATE
Definition vpRobot.h:78
@ TOOL_FRAME
Definition vpRobot.h:82
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
static const double maxRotationVelocityDefault
Definition vpRobot.h:97
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition vpRobot.h:106
double maxTranslationVelocity
Definition vpRobot.h:94
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
double maxRotationVelocity
Definition vpRobot.h:96
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.