vpViper.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface for a  generic ADEPT Viper (either 650 or 850) robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <cmath>  // std::fabs
#include <limits> // numeric_limits
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpViper.h>

const unsigned int vpViper::njoint = 6;

vpViper::vpViper()
  : eMc(), etc(), erc(), a1(0), d1(0), a2(), a3(), d4(0), d6(0), d7(0), c56(0), joint_max(), joint_min()
{
  // Default values are initialized

  // Denavit Hartenberg parameters
  a1 = 0.075;
  a2 = 0.365;
  a3 = 0.090;
  d1 = 0.335;
  d4 = 0.405;
  d6 = 0.080;
  d7 = 0.0666;
  c56 = -341.33 / 9102.22;

  // Software joint limits in radians
  joint_min.resize(njoint);
  joint_min[0] = vpMath::rad(-170);
  joint_min[1] = vpMath::rad(-190);
  joint_min[2] = vpMath::rad(-29);
  joint_min[3] = vpMath::rad(-190);
  joint_min[4] = vpMath::rad(-120);
  joint_min[5] = vpMath::rad(-360);
  joint_max.resize(njoint);
  joint_max[0] = vpMath::rad(170);
  joint_max[1] = vpMath::rad(45);
  joint_max[2] = vpMath::rad(256);
  joint_max[3] = vpMath::rad(190);
  joint_max[4] = vpMath::rad(120);
  joint_max[5] = vpMath::rad(360);

  // End effector to camera transformation
  eMc.eye();
}

vpHomogeneousMatrix vpViper::getForwardKinematics(const vpColVector &q) const
{
  vpHomogeneousMatrix fMc;
  fMc = get_fMc(q);

  return fMc;
}

bool vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod,
                                           const bool &verbose) const
{
  double eps = 0.01;
  if (q >= joint_min[joint] - eps && q <= joint_max[joint] + eps) {
    q_mod = q;
    return true;
  }

  q_mod = q + 2 * M_PI;
  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
    return true;
  }

  q_mod = q - 2 * M_PI;
  if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
    return true;
  }

  if (verbose) {
    std::cout << "Joint " << joint << " not in limits: " << this->joint_min[joint] << " < " << q << " < "
              << this->joint_max[joint] << std::endl;
  }

  return false;
}

unsigned int vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q,
                                                const bool &verbose) const
{
  vpColVector q_sol[8];

  for (unsigned int i = 0; i < 8; i++)
    q_sol[i].resize(6);

  double c1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double c3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double c23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double c4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double c5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double c6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double s6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  bool ok[8];

  if (q.getRows() != njoint)
    q.resize(6);

  for (unsigned int i = 0; i < 8; i++)
    ok[i] = true;

  double px = fMw[0][3]; // a*c1
  double py = fMw[1][3]; // a*s1
  double pz = fMw[2][3];

  // Compute q1
  double a_2 = px * px + py * py;
  // if (a_2 == 0) {// singularity
  if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) { // singularity
    c1[0] = cos(q[0]);
    s1[0] = sin(q[0]);
    c1[4] = cos(q[0] + M_PI);
    s1[4] = sin(q[0] + M_PI);
  } else {
    double a = sqrt(a_2);
    c1[0] = px / a;
    s1[0] = py / a;
    c1[4] = -px / a;
    s1[4] = -py / a;
  }

  double q1_mod;
  for (unsigned int i = 0; i < 8; i += 4) {
    q_sol[i][0] = atan2(s1[i], c1[i]);
    if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
      q_sol[i][0] = q1_mod;
      for (unsigned int j = 1; j < 4; j++) {
        c1[i + j] = c1[i];
        s1[i + j] = s1[i];
        q_sol[i + j][0] = q_sol[i][0];
      }
    } else {
      for (unsigned int j = 1; j < 4; j++)
        ok[i + j] = false;
    }
  }

  // Compute q3
  double K, q3_mod;
  for (unsigned int i = 0; i < 8; i += 4) {
    if (ok[i] == true) {
      K = (px * px + py * py + pz * pz + a1 * a1 - a2 * a2 - a3 * a3 + d1 * d1 - d4 * d4 -
           2 * (a1 * c1[i] * px + a1 * s1[i] * py + d1 * pz)) /
          (2 * a2);
      double d4_a3_K = d4 * d4 + a3 * a3 - K * K;

      q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
      q_sol[i + 2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));

      for (unsigned int j = 0; j < 4; j += 2) {
        if (d4_a3_K < 0) {
          for (unsigned int k = 0; k < 2; k++)
            ok[i + j + k] = false;
        } else {
          if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) == true) {
            for (unsigned int k = 0; k < 2; k++) {
              q_sol[i + j + k][2] = q3_mod;
              c3[i + j + k] = cos(q3_mod);
              s3[i + j + k] = sin(q3_mod);
            }
          } else {
            for (unsigned int k = 0; k < 2; k++)
              ok[i + j + k] = false;
          }
        }
      }
    }
  }
  //   std::cout << "ok apres q3: ";
  //   for (unsigned int i=0; i< 8; i++)
  //     std::cout << ok[i] << " ";
  //   std::cout << std::endl;

  // Compute q2
  double q23[8], q2_mod;
  for (unsigned int i = 0; i < 8; i += 2) {
    if (ok[i] == true) {
      // Compute q23 = q2+q3
      c23[i] = (-(a3 - a2 * c3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (d4 + a2 * s3[i])) /
               ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
      s23[i] = ((d4 + a2 * s3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (a3 - a2 * c3[i])) /
               ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
      q23[i] = atan2(s23[i], c23[i]);
      // std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] <<
      // std::endl;
      // q2 = q23 - q3
      q_sol[i][1] = q23[i] - q_sol[i][2];

      if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
        for (unsigned int j = 0; j < 2; j++) {
          q_sol[i + j][1] = q2_mod;
          c23[i + j] = c23[i];
          s23[i + j] = s23[i];
        }
      } else {
        for (unsigned int j = 0; j < 2; j++)
          ok[i + j] = false;
      }
    }
  }
  //   std::cout << "ok apres q2: ";
  //   for (unsigned int i=0; i< 8; i++)
  //     std::cout << ok[i] << " ";
  //   std::cout << std::endl;

  // Compute q4 as long as s5 != 0
  double r13 = fMw[0][2];
  double r23 = fMw[1][2];
  double r33 = fMw[2][2];
  double s4s5, c4s5, q4_mod, q5_mod;
  for (unsigned int i = 0; i < 8; i += 2) {
    if (ok[i] == true) {
      s4s5 = -s1[i] * r13 + c1[i] * r23;
      c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
      if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
        // s5 = 0
        c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
        // std::cout << "Singularity: s5 near 0: ";
        if (c5[i] > 0.)
          q_sol[i][4] = 0.0;
        else
          q_sol[i][4] = M_PI;

        if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
          for (unsigned int j = 0; j < 2; j++) {
            q_sol[i + j][3] = q[3]; // keep current q4
            q_sol[i + j][4] = q5_mod;
            c4[i] = cos(q_sol[i + j][3]);
            s4[i] = sin(q_sol[i + j][3]);
          }
        } else {
          for (unsigned int j = 0; j < 2; j++)
            ok[i + j] = false;
        }
      } else {
// s5 != 0
#if 0 // Modified 2016/03/10 since if and else are the same
      // if (c4s5 == 0) {
        if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
          // c4 = 0
          //  vpTRACE("c4 = 0");
          // q_sol[i][3] = q[3]; // keep current position
          q_sol[i][3] = atan2(s4s5, c4s5);
        }
        else {
          q_sol[i][3] = atan2(s4s5, c4s5);
        }
#else
        q_sol[i][3] = atan2(s4s5, c4s5);
#endif
        if (convertJointPositionInLimits(3, q_sol[i][3], q4_mod, verbose) == true) {
          q_sol[i][3] = q4_mod;
          c4[i] = cos(q4_mod);
          s4[i] = sin(q4_mod);
        } else {
          ok[i] = false;
        }
        if (q_sol[i][3] > 0.)
          q_sol[i + 1][3] = q_sol[i][3] + M_PI;
        else
          q_sol[i + 1][3] = q_sol[i][3] - M_PI;
        if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) == true) {
          q_sol[i + 1][3] = q4_mod;
          c4[i + 1] = cos(q4_mod);
          s4[i + 1] = sin(q4_mod);
        } else {
          ok[i + 1] = false;
        }

        // Compute q5
        for (unsigned int j = 0; j < 2; j++) {
          if (ok[i + j] == true) {
            c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
            s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
                        (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
                        s23[i + j] * c4[i + j] * r33;

            q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
            if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) {
              q_sol[i + j][4] = q5_mod;
            } else {

              ok[i + j] = false;
            }
          }
        }
      }
    }
  }

  // Compute q6
  // 4 solutions for q6 and 4 more solutions by flipping the wrist (see below)
  double r12 = fMw[0][1];
  double r22 = fMw[1][1];
  double r32 = fMw[2][1];
  double q6_mod;
  for (unsigned int i = 0; i < 8; i++) {
    c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
            s23[i] * s4[i] * r32;
    s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
            (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
            (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;

    q_sol[i][5] = atan2(s6[i], c6[i]);
    if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
      q_sol[i][5] = q6_mod;
    } else {
      ok[i] = false;
    }
  }

  // Select the best config in terms of distance from the current position
  unsigned int nbsol = 0;
  unsigned int sol = 0;
  vpColVector dist(8);
  for (unsigned int i = 0; i < 8; i++) {
    if (ok[i] == true) {
      nbsol++;
      sol = i;
      //      dist[i] = vpColVector::distance(q, q_sol[i]);
      vpColVector weight(6);
      weight = 1;
      weight[0] = 8;
      weight[1] = weight[2] = 4;
      dist[i] = 0;
      for (unsigned int j = 0; j < 6; j++) {
        double rought_dist = q[j] - q_sol[i][j];
        double modulo_dist = rought_dist;
        if (rought_dist > 0) {
          if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
            modulo_dist = rought_dist - 2 * M_PI;
        } else {
          if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
            modulo_dist = rought_dist + 2 * M_PI;
        }
        // std::cout << "dist " << i << ": " << rought_dist << " modulo: " <<
        // modulo_dist << std::endl;
        dist[i] += weight[j] * vpMath::sqr(modulo_dist);
      }
    }
    //  std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] <<
    //  " q: " << q_sol[i].t() << std::endl;
  }
  // std::cout << "dist: " << dist.t() << std::endl;
  if (nbsol) {
    for (unsigned int i = 0; i < 8; i++) {
      if (ok[i] == true)
        if (dist[i] < dist[sol])
          sol = i;
    }
    // Update the inverse kinematics solution
    q = q_sol[sol];

    //     std::cout << "Nearest solution (" << sol << ") with distance ("
    //        << dist[sol] << "): " << q_sol[sol].t() << std::endl;
  }
  return nbsol;
}

unsigned int vpViper::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose) const
{
  vpHomogeneousMatrix fMw;
  vpHomogeneousMatrix wMe;
  vpHomogeneousMatrix eMc_;
  this->get_wMe(wMe);
  this->get_eMc(eMc_);
  fMw = fMc * eMc_.inverse() * wMe.inverse();

  return (getInverseKinematicsWrist(fMw, q, verbose));
}

vpHomogeneousMatrix vpViper::get_fMc(const vpColVector &q) const
{
  vpHomogeneousMatrix fMc;
  get_fMc(q, fMc);

  return fMc;
}

void vpViper::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{

  // Compute the direct geometric model: fMe = transformation between
  // fix and end effector frame.
  vpHomogeneousMatrix fMe;

  get_fMe(q, fMe);

  fMc = fMe * this->eMc;

  return;
}

void vpViper::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
{
  double q1 = q[0];
  double q2 = q[1];
  double q3 = q[2];
  double q4 = q[3];
  double q5 = q[4];
  double q6 = q[5];
  //  We turn off the coupling since the measured positions are joint position
  //  taking into account the coupling factor. The coupling factor is relevant
  //  if positions are motor position.
  // double q6 = q[5] + c56 * q[4];

  //   std::cout << "q6 motor: " << q[5] << " rad "
  //        << vpMath::deg(q[5]) << " deg" << std::endl;
  //   std::cout << "q6 joint: " << q6 << " rad "
  //        << vpMath::deg(q6) << " deg" << std::endl;

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c2 = cos(q2);
  double s2 = sin(q2);
  // double c3 = cos(q3);
  // double s3 = sin(q3);
  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);
  double c6 = cos(q6);
  double s6 = sin(q6);
  double c23 = cos(q2 + q3);
  double s23 = sin(q2 + q3);

  fMe[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
  fMe[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
  fMe[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;

  fMe[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
  fMe[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
  fMe[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;

  fMe[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
  fMe[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
  fMe[2][2] = -s23 * c4 * s5 + c23 * c5;

  fMe[0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
  fMe[1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
  fMe[2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;

  // std::cout << "Effector position fMe: " << std::endl << fMe;

  return;
}
void vpViper::get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
{
  double q1 = q[0];
  double q2 = q[1];
  double q3 = q[2];
  double q4 = q[3];
  double q5 = q[4];
  double q6 = q[5];
  //  We turn off the coupling since the measured positions are joint position
  //  taking into account the coupling factor. The coupling factor is relevant
  //  if positions are motor position.
  // double q6 = q[5] + c56 * q[4];

  //   std::cout << "q6 motor: " << q[5] << " rad "
  //        << vpMath::deg(q[5]) << " deg" << std::endl;
  //   std::cout << "q6 joint: " << q6 << " rad "
  //        << vpMath::deg(q6) << " deg" << std::endl;

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c2 = cos(q2);
  double s2 = sin(q2);
  //  double c3 = cos(q3);
  // double s3 = sin(q3);
  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);
  double c6 = cos(q6);
  double s6 = sin(q6);
  double c23 = cos(q2 + q3);
  double s23 = sin(q2 + q3);

  fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
  fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
  fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;

  fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
  fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
  fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;

  fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
  fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
  fMw[2][2] = -s23 * c4 * s5 + c23 * c5;

  fMw[0][3] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
  fMw[1][3] = s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
  fMw[2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;

  // std::cout << "Wrist position fMw: " << std::endl << fMw;

  return;
}

void vpViper::get_wMe(vpHomogeneousMatrix &wMe) const
{
  // Set the rotation as identity
  wMe.eye();

  // Set the translation
  wMe[2][3] = d6;
}

void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }

void vpViper::get_eMs(vpHomogeneousMatrix &eMs) const
{
  eMs.eye();
  eMs[2][3] = -d7; // tz = -d7
}

void vpViper::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->eMc.inverse(); }

void vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
{
  vpHomogeneousMatrix cMe;
  get_cMe(cMe);

  cVe.buildFrom(cMe);

  return;
}

void vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
  vpMatrix V(6, 6);
  V = 0;
  // Compute the first and last block of V
  vpHomogeneousMatrix fMw;
  get_fMw(q, fMw);
  vpRotationMatrix fRw;
  fMw.extract(fRw);
  vpRotationMatrix wRf;
  wRf = fRw.inverse();
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      V[i][j] = V[i + 3][j + 3] = wRf[i][j];
    }
  }
  // Compute the second block of V
  vpHomogeneousMatrix wMe;
  get_wMe(wMe);
  vpHomogeneousMatrix eMw;
  eMw = wMe.inverse();
  vpTranslationVector etw;
  eMw.extract(etw);
  vpMatrix block2 = etw.skew() * wRf;
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      V[i][j + 3] = block2[i][j];
    }
  }
  // Compute eJe
  vpMatrix fJw;
  get_fJw(q, fJw);
  eJe = V * fJw;

  return;
}

void vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
{
  double q1 = q[0];
  double q2 = q[1];
  double q3 = q[2];
  double q4 = q[3];
  double q5 = q[4];

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c2 = cos(q2);
  double s2 = sin(q2);
  double c3 = cos(q3);
  double s3 = sin(q3);
  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);
  double c23 = cos(q2 + q3);
  double s23 = sin(q2 + q3);

  vpColVector J1(6);
  vpColVector J2(6);
  vpColVector J3(6);
  vpColVector J4(6);
  vpColVector J5(6);
  vpColVector J6(6);

  // Jacobian when d6 is set to zero
  J1[0] = -s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
  J1[1] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
  J1[2] = 0;
  J1[3] = 0;
  J1[4] = 0;
  J1[5] = 1;

  J2[0] = c1 * (s23 * a3 + c23 * d4 - a2 * s2);
  J2[1] = s1 * (s23 * a3 + c23 * d4 - a2 * s2);
  J2[2] = c23 * a3 - s23 * d4 - a2 * c2;
  J2[3] = -s1;
  J2[4] = c1;
  J2[5] = 0;

  J3[0] = c1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
  J3[1] = s1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
  J3[2] = -a3 * (s2 * s3 - c2 * c3) - d4 * (s2 * c3 + c2 * s3);
  J3[3] = -s1;
  J3[4] = c1;
  J3[5] = 0;

  J4[0] = 0;
  J4[1] = 0;
  J4[2] = 0;
  J4[3] = c1 * s23;
  J4[4] = s1 * s23;
  J4[5] = c23;

  J5[0] = 0;
  J5[1] = 0;
  J5[2] = 0;
  J5[3] = -c23 * c1 * s4 - s1 * c4;
  J5[4] = c1 * c4 - c23 * s1 * s4;
  J5[5] = s23 * s4;

  J6[0] = 0;
  J6[1] = 0;
  J6[2] = 0;
  J6[3] = (c1 * c23 * c4 - s1 * s4) * s5 + c1 * s23 * c5;
  J6[4] = (s1 * c23 * c4 + c1 * s4) * s5 + s1 * s23 * c5;
  J6[5] = -s23 * c4 * s5 + c23 * c5;

  fJw.resize(6, 6);
  for (unsigned int i = 0; i < 6; i++) {
    fJw[i][0] = J1[i];
    fJw[i][1] = J2[i];
    fJw[i][2] = J3[i];
    fJw[i][3] = J4[i];
    fJw[i][4] = J5[i];
    fJw[i][5] = J6[i];
  }
  return;
}
void vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
  vpMatrix V(6, 6);
  V = 0;
  // Set the first and last block to identity
  for (unsigned int i = 0; i < 6; i++)
    V[i][i] = 1;

  // Compute the second block of V
  vpHomogeneousMatrix fMw;
  get_fMw(q, fMw);
  vpRotationMatrix fRw;
  fMw.extract(fRw);
  vpHomogeneousMatrix wMe;
  get_wMe(wMe);
  vpHomogeneousMatrix eMw;
  eMw = wMe.inverse();
  vpTranslationVector etw;
  eMw.extract(etw);
  vpMatrix block2 = (fRw * etw).skew();
  // Set the second block
  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      V[i][j + 3] = block2[i][j];

  // Compute fJe
  vpMatrix fJw;
  get_fJw(q, fJw);
  fJe = V * fJw;

  return;
}

vpColVector vpViper::getJointMin() const { return joint_min; }

vpColVector vpViper::getJointMax() const { return joint_max; }

double vpViper::getCoupl56() const { return c56; }

void vpViper::set_eMc(const vpHomogeneousMatrix &eMc_)
{
  this->eMc = eMc_;
  this->eMc.extract(etc);
  vpRotationMatrix R(this->eMc);
  this->erc.buildFrom(R);
}

void vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
{
  this->etc = etc_;
  this->erc = erc_;
  vpRotationMatrix eRc(erc);
  this->eMc.buildFrom(etc, eRc);
}

VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper)
{
  vpRotationMatrix eRc;
  viper.eMc.extract(eRc);
  vpRxyzVector rxyz(eRc);

  // Convert joint limits in degrees
  vpColVector jmax = viper.joint_max;
  vpColVector jmin = viper.joint_min;
  jmax.rad2deg();
  jmin.rad2deg();

  os << "Joint Max (deg):" << std::endl
     << "\t" << jmax.t() << std::endl

     << "Joint Min (deg): " << std::endl
     << "\t" << jmin.t() << std::endl

     << "Coupling 5-6:" << std::endl
     << "\t" << viper.c56 << std::endl

     << "eMc: " << std::endl
     << "\tTranslation (m): " << viper.eMc[0][3] << " " << viper.eMc[1][3] << " " << viper.eMc[2][3] << "\t"
     << std::endl
     << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
     << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
     << "\t" << std::endl;

  return os;
}