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);
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);
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"));
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 == 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)) {
771 std::cout <<
"Plugin successfully loaded" << std::endl;