49 #include <visp3/core/vpCameraParameters.h> 50 #include <visp3/core/vpDebug.h> 51 #include <visp3/core/vpRotationMatrix.h> 52 #include <visp3/core/vpRxyzVector.h> 53 #include <visp3/core/vpTranslationVector.h> 54 #include <visp3/core/vpVelocityTwistMatrix.h> 55 #include <visp3/robot/vpRobotException.h> 56 #include <visp3/robot/vpViper.h> 66 : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min()
78 c56 = -341.33 / 9102.22;
143 bool vpViper::convertJointPositionInLimits(
unsigned int joint,
const double &q,
double &q_mod,
144 const bool &verbose)
const 152 q_mod = q + 2 * M_PI;
157 q_mod = q - 2 * M_PI;
163 std::cout <<
"Joint " << joint <<
" not in limits: " << this->
joint_min[joint] <<
" < " << q <<
" < " 223 const bool &verbose)
const 227 for (
unsigned int i = 0; i < 8; i++)
230 double c1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
231 double s1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
232 double c3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
233 double s3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
234 double c23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
235 double s23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
236 double c4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
237 double s4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
238 double c5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
239 double s5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
240 double c6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
241 double s6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
248 for (
unsigned int i = 0; i < 8; i++)
251 double px = fMw[0][3];
252 double py = fMw[1][3];
253 double pz = fMw[2][3];
256 double a_2 = px * px + py * py;
258 if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {
261 c1[4] = cos(q[0] + M_PI);
262 s1[4] = sin(q[0] + M_PI);
264 double a = sqrt(a_2);
272 for (
unsigned int i = 0; i < 8; i += 4) {
273 q_sol[i][0] = atan2(s1[i], c1[i]);
274 if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) ==
true) {
275 q_sol[i][0] = q1_mod;
276 for (
unsigned int j = 1; j < 4; j++) {
279 q_sol[i + j][0] = q_sol[i][0];
282 for (
unsigned int j = 1; j < 4; j++)
289 for (
unsigned int i = 0; i < 8; i += 4) {
292 2 * (
a1 * c1[i] * px +
a1 * s1[i] * py +
d1 * pz)) /
294 double d4_a3_K =
d4 *
d4 +
a3 *
a3 - K * K;
296 q_sol[i][2] = atan2(
a3,
d4) + atan2(K, sqrt(d4_a3_K));
297 q_sol[i + 2][2] = atan2(
a3,
d4) + atan2(K, -sqrt(d4_a3_K));
299 for (
unsigned int j = 0; j < 4; j += 2) {
301 for (
unsigned int k = 0; k < 2; k++)
302 ok[i + j + k] =
false;
304 if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) ==
true) {
305 for (
unsigned int k = 0; k < 2; k++) {
306 q_sol[i + j + k][2] = q3_mod;
307 c3[i + j + k] = cos(q3_mod);
308 s3[i + j + k] = sin(q3_mod);
311 for (
unsigned int k = 0; k < 2; k++)
312 ok[i + j + k] =
false;
324 double q23[8], q2_mod;
325 for (
unsigned int i = 0; i < 8; i += 2) {
328 c23[i] = (-(
a3 -
a2 * c3[i]) * (c1[i] * px + s1[i] * py -
a1) - (
d1 - pz) * (
d4 +
a2 * s3[i])) /
329 ((c1[i] * px + s1[i] * py -
a1) * (c1[i] * px + s1[i] * py -
a1) + (
d1 - pz) * (
d1 - pz));
330 s23[i] = ((
d4 +
a2 * s3[i]) * (c1[i] * px + s1[i] * py -
a1) - (
d1 - pz) * (
a3 -
a2 * c3[i])) /
331 ((c1[i] * px + s1[i] * py -
a1) * (c1[i] * px + s1[i] * py -
a1) + (
d1 - pz) * (
d1 - pz));
332 q23[i] = atan2(s23[i], c23[i]);
336 q_sol[i][1] = q23[i] - q_sol[i][2];
338 if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) ==
true) {
339 for (
unsigned int j = 0; j < 2; j++) {
340 q_sol[i + j][1] = q2_mod;
345 for (
unsigned int j = 0; j < 2; j++)
356 double r13 = fMw[0][2];
357 double r23 = fMw[1][2];
358 double r33 = fMw[2][2];
359 double s4s5, c4s5, q4_mod, q5_mod;
360 for (
unsigned int i = 0; i < 8; i += 2) {
362 s4s5 = -s1[i] * r13 + c1[i] * r23;
363 c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
366 c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
373 if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) ==
true) {
374 for (
unsigned int j = 0; j < 2; j++) {
375 q_sol[i + j][3] = q[3];
376 q_sol[i + j][4] = q5_mod;
377 c4[i] = cos(q_sol[i + j][3]);
378 s4[i] = sin(q_sol[i + j][3]);
381 for (
unsigned int j = 0; j < 2; j++)
386 #if 0 // Modified 2016/03/10 since if and else are the same 388 if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
392 q_sol[i][3] = atan2(s4s5, c4s5);
395 q_sol[i][3] = atan2(s4s5, c4s5);
398 q_sol[i][3] = atan2(s4s5, c4s5);
400 if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) ==
true) {
401 q_sol[i][3] = q4_mod;
407 if (q_sol[i][3] > 0.)
408 q_sol[i + 1][3] = q_sol[i][3] + M_PI;
410 q_sol[i + 1][3] = q_sol[i][3] - M_PI;
411 if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) ==
true) {
412 q_sol[i + 1][3] = q4_mod;
413 c4[i + 1] = cos(q4_mod);
414 s4[i + 1] = sin(q4_mod);
420 for (
unsigned int j = 0; j < 2; j++) {
421 if (ok[i + j] ==
true) {
422 c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
423 s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
424 (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
425 s23[i + j] * c4[i + j] * r33;
427 q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
428 if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) ==
true) {
429 q_sol[i + j][4] = q5_mod;
442 double r12 = fMw[0][1];
443 double r22 = fMw[1][1];
444 double r32 = fMw[2][1];
446 for (
unsigned int i = 0; i < 8; i++) {
447 c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
448 s23[i] * s4[i] * r32;
449 s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
450 (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
451 (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
453 q_sol[i][5] = atan2(s6[i], c6[i]);
454 if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) ==
true) {
455 q_sol[i][5] = q6_mod;
462 unsigned int nbsol = 0;
463 unsigned int sol = 0;
465 for (
unsigned int i = 0; i < 8; i++) {
473 weight[1] = weight[2] = 4;
475 for (
unsigned int j = 0; j < 6; j++) {
476 double rought_dist = q[j] - q_sol[i][j];
477 double modulo_dist = rought_dist;
478 if (rought_dist > 0) {
479 if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
480 modulo_dist = rought_dist - 2 * M_PI;
482 if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
483 modulo_dist = rought_dist + 2 * M_PI;
495 for (
unsigned int i = 0; i < 8; i++) {
497 if (dist[i] < dist[sol])
638 fMc = fMe * this->
eMc;
745 double c23 = cos(q2 + q3);
746 double s23 = sin(q2 + q3);
748 fMe[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
749 fMe[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
750 fMe[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
752 fMe[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
753 fMe[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
754 fMe[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
756 fMe[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
757 fMe[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
758 fMe[2][2] = -s23 * c4 * s5 + c23 * c5;
760 fMe[0][3] = c1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) - s1 * s4 * s5 *
d6;
761 fMe[1][3] = s1 * (c23 * (c4 * s5 *
d6 -
a3) + s23 * (c5 *
d6 +
d4) +
a1 +
a2 * c2) + c1 * s4 * s5 *
d6;
762 fMe[2][3] = s23 * (
a3 - c4 * s5 *
d6) + c23 * (c5 *
d6 +
d4) -
a2 * s2 +
d1;
840 double c23 = cos(q2 + q3);
841 double s23 = sin(q2 + q3);
843 fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
844 fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
845 fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
847 fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
848 fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
849 fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
851 fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
852 fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
853 fMw[2][2] = -s23 * c4 * s5 + c23 * c5;
855 fMw[0][3] = c1 * (-c23 *
a3 + s23 *
d4 +
a1 +
a2 * c2);
856 fMw[1][3] = s1 * (-c23 *
a3 + s23 *
d4 +
a1 +
a2 * c2);
857 fMw[2][3] = s23 *
a3 + c23 *
d4 -
a2 * s2 +
d1;
981 for (
unsigned int i = 0; i < 3; i++) {
982 for (
unsigned int j = 0; j < 3; j++) {
983 V[i][j] = V[i + 3][j + 3] = wRf[i][j];
994 for (
unsigned int i = 0; i < 3; i++) {
995 for (
unsigned int j = 0; j < 3; j++) {
996 V[i][j + 3] = block2[i][j];
1062 double c1 = cos(q1);
1063 double s1 = sin(q1);
1064 double c2 = cos(q2);
1065 double s2 = sin(q2);
1066 double c3 = cos(q3);
1067 double s3 = sin(q3);
1068 double c4 = cos(q4);
1069 double s4 = sin(q4);
1070 double c5 = cos(q5);
1071 double s5 = sin(q5);
1072 double c23 = cos(q2 + q3);
1073 double s23 = sin(q2 + q3);
1083 J1[0] = -s1 * (-c23 *
a3 + s23 *
d4 +
a1 +
a2 * c2);
1084 J1[1] = c1 * (-c23 *
a3 + s23 *
d4 +
a1 +
a2 * c2);
1090 J2[0] = c1 * (s23 *
a3 + c23 *
d4 -
a2 * s2);
1091 J2[1] = s1 * (s23 *
a3 + c23 *
d4 -
a2 * s2);
1092 J2[2] = c23 *
a3 - s23 *
d4 -
a2 * c2;
1097 J3[0] = c1 * (
a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) *
d4);
1098 J3[1] = s1 * (
a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) *
d4);
1099 J3[2] = -
a3 * (s2 * s3 - c2 * c3) -
d4 * (s2 * c3 + c2 * s3);
1114 J5[3] = -c23 * c1 * s4 - s1 * c4;
1115 J5[4] = c1 * c4 - c23 * s1 * s4;
1121 J6[3] = (c1 * c23 * c4 - s1 * s4) * s5 + c1 * s23 * c5;
1122 J6[4] = (s1 * c23 * c4 + c1 * s4) * s5 + s1 * s23 * c5;
1123 J6[5] = -s23 * c4 * s5 + c23 * c5;
1126 for (
unsigned int i = 0; i < 6; i++) {
1164 for (
unsigned int i = 0; i < 6; i++)
1178 vpMatrix block2 = (fRw * etw).skew();
1180 for (
unsigned int i = 0; i < 3; i++)
1181 for (
unsigned int j = 0; j < 3; j++)
1182 V[i][j + 3] = block2[i][j];
1277 os <<
"Joint Max (deg):" << std::endl
1278 <<
"\t" << jmax.
t() << std::endl
1280 <<
"Joint Min (deg): " << std::endl
1281 <<
"\t" << jmin.
t() << std::endl
1283 <<
"Coupling 5-6:" << std::endl
1284 <<
"\t" << viper.
c56 << std::endl
1286 <<
"eMc: " << std::endl
1287 <<
"\tTranslation (m): " << viper.
eMc[0][3] <<
" " << viper.
eMc[1][3] <<
" " << viper.
eMc[2][3] <<
"\t" 1289 <<
"\tRotation Rxyz (rad) : " << rxyz[0] <<
" " << rxyz[1] <<
" " << rxyz[2] <<
"\t" << std::endl
1291 <<
"\t" << std::endl;
void get_cVe(vpVelocityTwistMatrix &cVe) const
Implementation of a matrix and operations on matrices.
double d7
for force/torque location
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
vpHomogeneousMatrix eMc
End effector to camera transformation.
vpRotationMatrix inverse() const
Modelisation of the ADEPT Viper robot.
double getCoupl56() const
unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false) const
void get_wMe(vpHomogeneousMatrix &wMe) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpViper &viper)
void get_eMc(vpHomogeneousMatrix &eMc) const
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
void get_eMs(vpHomogeneousMatrix &eMs) const
Implementation of a rotation matrix and operations on such kind of matrices.
double c56
Mechanical coupling between joint 5 and joint 6.
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
void get_fJw(const vpColVector &q, vpMatrix &fJw) const
unsigned int getRows() const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpColVector getJointMin() const
void get_cMe(vpHomogeneousMatrix &cMe) const
static double deg(double rad)
vpColVector getJointMax() const
Implementation of column vector and the associated operations.
vpHomogeneousMatrix inverse() const
Implementation of a rotation vector as Euler angle minimal representation.
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Class that consider the case of a translation vector.
static const unsigned int njoint
Number of joint.
void resize(const unsigned int i, const bool flagNullify=true)