39 #include <visp3/core/vpConfig.h>
41 #ifdef VISP_HAVE_JACOSDK
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotException.h>
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/robot/vpRobotKinova.h>
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()
82 if (dof == 4 || dof == 6 || dof == 7) {
86 "Unsupported Kinova Jaco degrees of freedom: %d. Possible values are 4, 6 or 7.", dof));
133 std::cout <<
"Not implemented ! " << std::endl;
153 std::cout <<
"Not implemented ! " << std::endl;
182 "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.
size()));
212 TrajectoryPoint pointToSend;
213 pointToSend.InitStruct();
215 pointToSend.Position.Type = CARTESIAN_VELOCITY;
216 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
218 pointToSend.Position.CartesianPosition.X =
static_cast<float>(v_mix[0]);
219 pointToSend.Position.CartesianPosition.Y =
static_cast<float>(v_mix[1]);
220 pointToSend.Position.CartesianPosition.Z =
static_cast<float>(v_mix[2]);
221 pointToSend.Position.CartesianPosition.ThetaX =
static_cast<float>(v_mix[3]);
222 pointToSend.Position.CartesianPosition.ThetaY =
static_cast<float>(v_mix[4]);
223 pointToSend.Position.CartesianPosition.ThetaZ =
static_cast<float>(v_mix[5]);
225 KinovaSetCartesianControl();
227 KinovaSendBasicTrajectory(pointToSend);
245 TrajectoryPoint pointToSend;
246 pointToSend.InitStruct();
248 pointToSend.Position.Type = CARTESIAN_VELOCITY;
249 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
251 pointToSend.Position.CartesianPosition.X =
static_cast<float>(v_mix[0]);
252 pointToSend.Position.CartesianPosition.Y =
static_cast<float>(v_mix[1]);
253 pointToSend.Position.CartesianPosition.Z =
static_cast<float>(v_mix[2]);
254 pointToSend.Position.CartesianPosition.ThetaX =
static_cast<float>(v_mix[3]);
255 pointToSend.Position.CartesianPosition.ThetaY =
static_cast<float>(v_mix[4]);
256 pointToSend.Position.CartesianPosition.ThetaZ =
static_cast<float>(v_mix[5]);
258 KinovaSetCartesianControl();
260 KinovaSendBasicTrajectory(pointToSend);
271 std::cout <<
"rotation matrix from base to endeffector is bRe : " << std::endl;
272 std::cout <<
"bRe:\n" << bRe << std::endl;
280 TrajectoryPoint pointToSend;
281 pointToSend.InitStruct();
283 pointToSend.Position.Type = CARTESIAN_VELOCITY;
284 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
286 pointToSend.Position.CartesianPosition.X =
static_cast<float>(v_mix[0]);
287 pointToSend.Position.CartesianPosition.Y =
static_cast<float>(v_mix[1]);
288 pointToSend.Position.CartesianPosition.Z =
static_cast<float>(v_mix[2]);
289 pointToSend.Position.CartesianPosition.ThetaX =
static_cast<float>(v_e[3]);
290 pointToSend.Position.CartesianPosition.ThetaY =
static_cast<float>(v_e[4]);
291 pointToSend.Position.CartesianPosition.ThetaZ =
static_cast<float>(v_e[5]);
293 KinovaSetCartesianControl();
294 KinovaSendBasicTrajectory(pointToSend);
310 if (qdot.
size() !=
static_cast<unsigned int>(
nDof)) {
312 "Cannot apply a %d-dim joint velocity vector to the Jaco robot configured with %d DoF",
315 TrajectoryPoint pointToSend;
316 pointToSend.InitStruct();
318 pointToSend.Position.Type = ANGULAR_VELOCITY;
319 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
322 pointToSend.Position.Actuators.Actuator7 =
static_cast<float>(
vpMath::deg(qdot[6]));
323 pointToSend.Position.Actuators.Actuator6 =
static_cast<float>(
vpMath::deg(qdot[5]));
324 pointToSend.Position.Actuators.Actuator5 =
static_cast<float>(
vpMath::deg(qdot[4]));
325 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(qdot[3]));
326 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(qdot[2]));
327 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(qdot[1]));
328 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(qdot[0]));
332 pointToSend.Position.Actuators.Actuator6 =
static_cast<float>(
vpMath::deg(qdot[5]));
333 pointToSend.Position.Actuators.Actuator5 =
static_cast<float>(
vpMath::deg(qdot[4]));
334 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(qdot[3]));
335 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(qdot[2]));
336 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(qdot[1]));
337 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(qdot[0]));
341 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(qdot[3]));
342 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(qdot[2]));
343 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(qdot[1]));
344 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(qdot[0]));
351 KinovaSetAngularControl();
353 KinovaSendBasicTrajectory(pointToSend);
383 "Cannot send a velocity to the robot. "
384 "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
385 "entering your control loop.");
397 if (vel.
size() != 6) {
399 "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.
size()));
403 for (
unsigned int i = 0; i < 3; i++)
405 for (
unsigned int i = 3; i < 6; i++)
416 if (vel.
size() !=
static_cast<size_t>(
nDof)) {
447 AngularPosition currentCommand;
450 KinovaGetAngularCommand(currentCommand);
455 q[6] =
vpMath::rad(currentCommand.Actuators.Actuator7);
456 q[5] =
vpMath::rad(currentCommand.Actuators.Actuator6);
457 q[4] =
vpMath::rad(currentCommand.Actuators.Actuator5);
458 q[3] =
vpMath::rad(currentCommand.Actuators.Actuator4);
459 q[2] =
vpMath::rad(currentCommand.Actuators.Actuator3);
460 q[1] =
vpMath::rad(currentCommand.Actuators.Actuator2);
461 q[0] =
vpMath::rad(currentCommand.Actuators.Actuator1);
465 q[5] =
vpMath::rad(currentCommand.Actuators.Actuator6);
466 q[4] =
vpMath::rad(currentCommand.Actuators.Actuator5);
467 q[3] =
vpMath::rad(currentCommand.Actuators.Actuator4);
468 q[2] =
vpMath::rad(currentCommand.Actuators.Actuator3);
469 q[1] =
vpMath::rad(currentCommand.Actuators.Actuator2);
470 q[0] =
vpMath::rad(currentCommand.Actuators.Actuator1);
474 q[3] =
vpMath::rad(currentCommand.Actuators.Actuator4);
475 q[2] =
vpMath::rad(currentCommand.Actuators.Actuator3);
476 q[1] =
vpMath::rad(currentCommand.Actuators.Actuator2);
477 q[0] =
vpMath::rad(currentCommand.Actuators.Actuator1);
525 CartesianPosition currentCommand;
527 KinovaGetCartesianCommand(currentCommand);
529 position[0] = currentCommand.Coordinates.X;
530 position[1] = currentCommand.Coordinates.Y;
531 position[2] = currentCommand.Coordinates.Z;
532 position[3] = currentCommand.Coordinates.ThetaX;
533 position[4] = currentCommand.Coordinates.ThetaY;
534 position[5] = currentCommand.Coordinates.ThetaZ;
536 std::cout <<
"Not implemented ! " << std::endl;
570 for (
unsigned int i = 0; i < 3; i++) {
571 pose[i] = position[i];
572 rxyz[i] = position[i + 3];
575 for (
unsigned int i = 0; i < 3; i++) {
595 if (
static_cast<int>(q.
size()) !=
nDof) {
597 "Cannot move robot to a joint position of dim %u that is not a %d-dim vector", q.
size(),
nDof));
599 TrajectoryPoint pointToSend;
600 pointToSend.InitStruct();
602 pointToSend.Position.Type = ANGULAR_POSITION;
603 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
606 pointToSend.Position.Actuators.Actuator7 =
static_cast<float>(
vpMath::deg(q[6]));
607 pointToSend.Position.Actuators.Actuator6 =
static_cast<float>(
vpMath::deg(q[5]));
608 pointToSend.Position.Actuators.Actuator5 =
static_cast<float>(
vpMath::deg(q[4]));
609 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(q[3]));
610 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(q[2]));
611 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(q[1]));
612 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(q[0]));
616 pointToSend.Position.Actuators.Actuator6 =
static_cast<float>(
vpMath::deg(q[5]));
617 pointToSend.Position.Actuators.Actuator5 =
static_cast<float>(
vpMath::deg(q[4]));
618 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(q[3]));
619 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(q[2]));
620 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(q[1]));
621 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(q[0]));
625 pointToSend.Position.Actuators.Actuator4 =
static_cast<float>(
vpMath::deg(q[3]));
626 pointToSend.Position.Actuators.Actuator3 =
static_cast<float>(
vpMath::deg(q[2]));
627 pointToSend.Position.Actuators.Actuator2 =
static_cast<float>(
vpMath::deg(q[1]));
628 pointToSend.Position.Actuators.Actuator1 =
static_cast<float>(
vpMath::deg(q[0]));
635 KinovaSetAngularControl();
638 std::cout <<
"Move robot to joint position [rad rad rad rad rad rad]: " << q.
t() << std::endl;
640 KinovaSendBasicTrajectory(pointToSend);
644 "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.
size()));
646 TrajectoryPoint pointToSend;
647 pointToSend.InitStruct();
649 pointToSend.Position.Type = CARTESIAN_POSITION;
650 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
651 pointToSend.Position.CartesianPosition.X =
static_cast<float>(q[0]);
652 pointToSend.Position.CartesianPosition.Y =
static_cast<float>(q[1]);
653 pointToSend.Position.CartesianPosition.Z =
static_cast<float>(q[2]);
654 pointToSend.Position.CartesianPosition.ThetaX =
static_cast<float>(q[3]);
655 pointToSend.Position.CartesianPosition.ThetaY =
static_cast<float>(q[4]);
656 pointToSend.Position.CartesianPosition.ThetaZ =
static_cast<float>(q[5]);
658 KinovaSetCartesianControl();
661 std::cout <<
"Move robot to cartesian position [m m m rad rad rad]: " << q.
t() << std::endl;
663 KinovaSendBasicTrajectory(pointToSend);
667 "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
688 std::cout <<
"Not implemented ! " << std::endl;
713 : std::string(
"Kinova.API.EthCommandLayerUbuntu.so");
716 std::cout <<
"Load plugin: \"" << plugin <<
"\"" << std::endl;
718 m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
722 KinovaCloseAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"CloseAPI")).c_str());
723 KinovaGetAngularCommand =
724 (int (*)(AngularPosition &))dlsym(m_command_layer_handle, (prefix + std::string(
"GetAngularCommand")).c_str());
725 KinovaGetCartesianCommand = (int (*)(CartesianPosition &))dlsym(
726 m_command_layer_handle, (prefix + std::string(
"GetCartesianCommand")).c_str());
727 KinovaGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE],
int &result))dlsym(
728 m_command_layer_handle, (prefix + std::string(
"GetDevices")).c_str());
729 KinovaInitAPI = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"InitAPI")).c_str());
730 KinovaInitFingers = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"InitFingers")).c_str());
731 KinovaMoveHome = (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"MoveHome")).c_str());
732 KinovaSendBasicTrajectory =
733 (int (*)(TrajectoryPoint))dlsym(m_command_layer_handle, (prefix + std::string(
"SendBasicTrajectory")).c_str());
734 KinovaSetActiveDevice =
735 (int (*)(KinovaDevice devices))dlsym(m_command_layer_handle, (prefix + std::string(
"SetActiveDevice")).c_str());
736 KinovaSetAngularControl =
737 (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"SetAngularControl")).c_str());
738 KinovaSetCartesianControl =
739 (int (*)())dlsym(m_command_layer_handle, (prefix + std::string(
"SetCartesianControl")).c_str());
743 : std::string(
"CommandLayerEthernet.dll");
746 std::cout <<
"Load plugin: \"" << plugin <<
"\"" << std::endl;
748 m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
751 KinovaCloseAPI = (int (*)())GetProcAddress(m_command_layer_handle,
"CloseAPI");
752 KinovaGetAngularCommand = (int (*)(AngularPosition &))GetProcAddress(m_command_layer_handle,
"GetAngularCommand");
753 KinovaGetCartesianCommand =
754 (int (*)(CartesianPosition &))GetProcAddress(m_command_layer_handle,
"GetCartesianCommand");
756 (int (*)(KinovaDevice[MAX_KINOVA_DEVICE],
int &))GetProcAddress(m_command_layer_handle,
"GetDevices");
757 KinovaInitAPI = (int (*)())GetProcAddress(m_command_layer_handle,
"InitAPI");
758 KinovaInitFingers = (int (*)())GetProcAddress(m_command_layer_handle,
"InitFingers");
759 KinovaMoveHome = (int (*)())GetProcAddress(m_command_layer_handle,
"MoveHome");
760 KinovaSendBasicTrajectory = (int (*)(TrajectoryPoint))GetProcAddress(m_command_layer_handle,
"SendBasicTrajectory");
761 KinovaSetActiveDevice = (int (*)(KinovaDevice))GetProcAddress(m_command_layer_handle,
"SetActiveDevice");
762 KinovaSetAngularControl = (int (*)())GetProcAddress(m_command_layer_handle,
"SetAngularControl");
763 KinovaSetCartesianControl = (int (*)())GetProcAddress(m_command_layer_handle,
"SetCartesianControl");
767 if ((KinovaCloseAPI == NULL) || (KinovaGetAngularCommand == NULL) || (KinovaGetAngularCommand == NULL) ||
768 (KinovaGetCartesianCommand == NULL) || (KinovaGetDevices == NULL) || (KinovaInitAPI == NULL) ||
769 (KinovaInitFingers == NULL) || (KinovaMoveHome == NULL) || (KinovaSendBasicTrajectory == NULL) ||
770 (KinovaSetActiveDevice == NULL) || (KinovaSetAngularControl == NULL) || (KinovaSetCartesianControl == NULL)) {
774 std::cout <<
"Plugin successfully loaded" << std::endl;
787 std::cout <<
"Close plugin" << std::endl;
790 dlclose(m_command_layer_handle);
792 FreeLibrary(m_command_layer_handle);
811 std::cout <<
"Move the robot to home position" << std::endl;
827 int result = (*KinovaInitAPI)();
830 std::cout <<
"Initialization's result: " << result << std::endl;
876 #elif !defined(VISP_BUILD_SHARED_LIBS)
879 void dummy_vpRobotKinova(){};
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Implementation of a pose vector and operations on poses.
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)
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
static const double maxTranslationVelocityDefault
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
static const double maxRotationVelocityDefault
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
double getMaxRotationVelocity(void) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
double maxTranslationVelocity
double getMaxTranslationVelocity(void) const
double maxRotationVelocity
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.