36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_JACOSDK
40 #include <visp3/core/vpIoTools.h>
41 #include <visp3/robot/vpRobotException.h>
48 #include <visp3/core/vpHomogeneousMatrix.h>
49 #include <visp3/robot/vpRobotKinova.h>
55 : m_eMc(), m_plugin_location(
"./"), m_verbose(false), m_plugin_loaded(false), m_devices_count(0),
56 m_devices_list(nullptr), m_active_device(-1), m_command_layer(CMD_LAYER_UNSET), m_command_layer_handle()
79 if (dof == 4 || dof == 6 || dof == 7) {
83 "Unsupported Kinova Jaco degrees of freedom: %d. Possible values are 4, 6 or 7.", dof));
130 std::cout <<
"Not implemented ! " << std::endl;
150 std::cout <<
"Not implemented ! " << std::endl;
179 "Cannot send a velocity twist vector in tool frame that is not 6-dim (%d)", v.
size()));
209 TrajectoryPoint pointToSend;
210 pointToSend.InitStruct();
212 pointToSend.Position.Type = CARTESIAN_VELOCITY;
213 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
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]);
222 KinovaSetCartesianControl();
224 KinovaSendBasicTrajectory(pointToSend);
242 TrajectoryPoint pointToSend;
243 pointToSend.InitStruct();
245 pointToSend.Position.Type = CARTESIAN_VELOCITY;
246 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
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]);
255 KinovaSetCartesianControl();
257 KinovaSendBasicTrajectory(pointToSend);
268 std::cout <<
"rotation matrix from base to endeffector is bRe : " << std::endl;
269 std::cout <<
"bRe:\n" << bRe << std::endl;
277 TrajectoryPoint pointToSend;
278 pointToSend.InitStruct();
280 pointToSend.Position.Type = CARTESIAN_VELOCITY;
281 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
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]);
290 KinovaSetCartesianControl();
291 KinovaSendBasicTrajectory(pointToSend);
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",
312 TrajectoryPoint pointToSend;
313 pointToSend.InitStruct();
315 pointToSend.Position.Type = ANGULAR_VELOCITY;
316 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
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]));
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]));
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]));
348 KinovaSetAngularControl();
350 KinovaSendBasicTrajectory(pointToSend);
380 "Cannot send a velocity to the robot. "
381 "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
382 "entering your control loop.");
394 if (vel.
size() != 6) {
396 "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.
size()));
400 for (
unsigned int i = 0; i < 3; i++)
402 for (
unsigned int i = 3; i < 6; i++)
413 if (vel.
size() !=
static_cast<size_t>(
nDof)) {
444 AngularPosition currentCommand;
447 KinovaGetAngularCommand(currentCommand);
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);
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);
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);
522 CartesianPosition currentCommand;
524 KinovaGetCartesianCommand(currentCommand);
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;
533 std::cout <<
"Not implemented ! " << std::endl;
567 for (
unsigned int i = 0; i < 3; i++) {
568 pose[i] = position[i];
569 rxyz[i] = position[i + 3];
572 for (
unsigned int i = 0; i < 3; i++) {
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));
596 TrajectoryPoint pointToSend;
597 pointToSend.InitStruct();
599 pointToSend.Position.Type = ANGULAR_POSITION;
600 pointToSend.Position.HandMode = HAND_NOMOVEMENT;
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]));
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]));
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]));
632 KinovaSetAngularControl();
635 std::cout <<
"Move robot to joint position [rad rad rad rad rad rad]: " << q.
t() << std::endl;
637 KinovaSendBasicTrajectory(pointToSend);
641 "Cannot move robot to cartesian position of dim %d that is not a 6-dim vector", q.
size()));
643 TrajectoryPoint pointToSend;
644 pointToSend.InitStruct();
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]);
655 KinovaSetCartesianControl();
658 std::cout <<
"Move robot to cartesian position [m m m rad rad rad]: " << q.
t() << std::endl;
660 KinovaSendBasicTrajectory(pointToSend);
664 "Cannot move robot to a cartesian position. Only joint positioning is implemented"));
685 std::cout <<
"Not implemented ! " << std::endl;
710 : std::string(
"Kinova.API.EthCommandLayerUbuntu.so");
713 std::cout <<
"Load plugin: \"" << plugin <<
"\"" << std::endl;
715 m_command_layer_handle = dlopen(plugin.c_str(), RTLD_NOW | RTLD_GLOBAL);
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());
740 : std::string(
"CommandLayerEthernet.dll");
743 std::cout <<
"Load plugin: \"" << plugin <<
"\"" << std::endl;
745 m_command_layer_handle = LoadLibrary(TEXT(plugin.c_str()));
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");
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");
764 if ((KinovaCloseAPI ==
nullptr) || (KinovaGetAngularCommand ==
nullptr) || (KinovaGetAngularCommand ==
nullptr) ||
765 (KinovaGetCartesianCommand ==
nullptr) || (KinovaGetDevices ==
nullptr) || (KinovaInitAPI ==
nullptr) ||
766 (KinovaInitFingers ==
nullptr) || (KinovaMoveHome ==
nullptr) || (KinovaSendBasicTrajectory ==
nullptr) ||
767 (KinovaSetActiveDevice ==
nullptr) || (KinovaSetAngularControl ==
nullptr) || (KinovaSetCartesianControl ==
nullptr)) {
771 std::cout <<
"Plugin successfully loaded" << std::endl;
784 std::cout <<
"Close plugin" << std::endl;
787 dlclose(m_command_layer_handle);
789 FreeLibrary(m_command_layer_handle);
808 std::cout <<
"Move the robot to home position" << std::endl;
824 int result = (*KinovaInitAPI)();
827 std::cout <<
"Initialization's result: " << result << std::endl;
873 #elif !defined(VISP_BUILD_SHARED_LIBS)
876 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 emitted 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 emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
KinovaDevice * m_devices_list
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override
void setDoF(unsigned int dof)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) override
std::string m_plugin_location
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
void get_fJe(vpMatrix &fJe) override
CommandLayer m_command_layer
void setJointVelocity(const vpColVector &qdot)
void getJointPosition(vpColVector &q)
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void get_eJe(vpMatrix &eJe) override
void setActiveDevice(int device)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) override
virtual ~vpRobotKinova() override
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.