Test Viper 650 robot joint and cartesian control.
#include <iostream>
#include <visp3/robot/vpRobotViper850.h>
#ifdef VISP_HAVE_VIPER850
{
for (unsigned int i = 0; i < 3; i++) {
if (std::fabs(t1[i] - t2[i]) > epsilon)
return false;
if (std::fabs(tu1[i] - tu2[i]) > epsilon)
return false;
}
return true;
}
{
for (
unsigned int i = 0; i < q1.
size(); i++) {
if (std::fabs(q1[i] - q2[i]) > epsilon) {
return false;
}
}
return true;
}
int main()
{
try {
#if 0
eMt[0][0] = 0;
eMt[1][0] = 0;
eMt[2][0] = -1;
eMt[0][1] = -sqrt(2)/2;
eMt[1][1] = -sqrt(2)/2;
eMt[2][1] = 0;
eMt[0][2] = -sqrt(2)/2;
eMt[1][2] = sqrt(2)/2;
eMt[2][2] = 0;
eMt[0][3] = -0.177;
eMt[1][3] = 0.177;
eMt[2][3] = 0.077;
#else
#endif
std::cout << "eMt:\n" << eMt << std::endl;
std::cout << "Connection to Viper 850 robot" << std::endl;
std::cout <<
"q: " << q.
t() << std::endl;
std::cout << "fMw:\n" << fMw << std::endl;
std::cout << "fMe:\n" << fMe << std::endl;
std::cout << "fMt:\n" << fMt << std::endl;
std::cout <<
"eMc:\n" << cMe.
inverse() << std::endl;
if (1) {
std::cout << "eMt_:\n" << eMt_ << std::endl;
std::cout << "Compare pose eMt and eMt_:" << std::endl;
if (!pose_equal(eMt, eMt_, 1e-4)) {
std::cout << " Error: Pose eMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
std::cout <<
"eMc:\n" << cMe.
inverse() << std::endl;
std::cout << "Compare pose eMt and eMc:" << std::endl;
if (!pose_equal(eMt, cMe.
inverse(), 1e-4)) {
std::cout << " Error: Pose eMc differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
}
if (1) {
for (unsigned int i = 0; i < 3; i++) {
f_t_t[i] = f_pose_t[i];
f_rxyz_t[i] = f_pose_t[i + 3];
}
std::cout << "fMt_ (from ref frame):\n" << fMt_ << std::endl;
std::cout << "Compare pose fMt and fMt_:" << std::endl;
if (!pose_equal(fMt, fMt_, 1e-4)) {
std::cout << " Error: Pose fMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
}
if (1) {
std::cout << "Move robot in joint (the robot should not move)" << std::endl;
std::cout <<
"Reach joint position q2: " << q2.
t() << std::endl;
std::cout << "Compare joint position q and q2:" << std::endl;
if (!joint_equal(q, q2, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
}
if (1) {
for (unsigned int i = 0; i < 3; i++) {
f_pose_t[i] = f_t_t[i];
f_pose_t[i + 3] = f_rxyz_t[i];
}
std::cout << "Move robot in reference frame (the robot should not move)" << std::endl;
std::cout <<
"Reach joint position q3: " << q3.
t() << std::endl;
std::cout << "Compare joint position q and q3:" << std::endl;
if (!joint_equal(q, q3, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
}
if (1) {
tMt[1][3] = 0.05;
std::cout << "fMt_:\n" << fMt_ << std::endl;
std::cout << "Move robot in joint position to reach fMt_" << std::endl;
std::cout << "Compare pose fMt_ and fpt_:" << std::endl;
std::cout << " Error: Pose fMt_ differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
}
if (0) {
v_t = 0;
std::cout << "Move robot in camera velocity" << std::endl;
}
}
if (0) {
v_t = 0;
std::cout << "Move robot in joint velocity" << std::endl;
}
}
if (1) {
v_t = 0;
std::cout << "Move robot in camera velocity" << std::endl;
}
}
if (1) {
v_t = 0;
std::cout << "Move robot in joint velocity" << std::endl;
}
}
if (1) {
tMt[1][3] = -0.05;
std::cout << "Move robot in joint position" << std::endl;
}
std::cout << "The end" << std::endl;
std::cout << "Test succeed" << std::endl;
std::cout <<
"Test failed with exception: " << e.
getMessage() << std::endl;
}
}
#else
int main()
{
std::cout << "The real Viper850 robot controller is not available." << std::endl;
return 0;
}
#endif