vpAfma4.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 the Irisa's Afma4 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#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/vpAfma4.h>
#include <visp3/robot/vpRobotException.h>

/* ----------------------------------------------------------------------- */
/* --- STATIC ------------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
const unsigned int vpAfma4::njoint = 4;

vpAfma4::vpAfma4() : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
{
  // Set the default parameters in case of the config files are not available.

  //
  // Geometric model constant parameters
  //
  this->_a1 = 0.205; // distance along x2
  this->_d3 = 0.403; // distance along z2
  this->_d4 = 0.14;  // distance along z3

  // Maximal value of the joints
  this->_joint_max[0] = 1.8;  // rad
  this->_joint_max[1] = 0.9;  // meter
  this->_joint_max[2] = 0.9;  // rad
  this->_joint_max[3] = 0.76; // rad
  // Minimal value of the joints
  this->_joint_min[0] = -1.5;  // rad
  this->_joint_min[1] = -0.9;  // meter
  this->_joint_min[2] = -3.5;  // rad
  this->_joint_min[3] = -0.76; // rad

  // Camera extrinsic parameters: effector to camera frame
  this->_etc[0] = 0.; // Translation
  this->_etc[1] = 0.;
  this->_etc[2] = 0.;
  this->_erc[0] = 0.; // Rotation
  this->_erc[1] = -M_PI / 2.;
  this->_erc[2] = 0;

  vpRotationMatrix eRc(_erc);
  this->_eMc.buildFrom(_etc, eRc);

  init();
}

void vpAfma4::init(void) { return; }

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

  return fMc;
}

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

  return fMc;
}

void vpAfma4::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 vpAfma4::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
{
  double q1 = q[0]; // rot touret
  double q2 = q[1]; // vertical translation
  double q4 = q[2]; // pan
  double q5 = q[3]; // tilt

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);

  /* Calcul du modele d'apres les angles. */
  fMe[0][0] = c1 * s4 * c5 + s1 * c4 * c5;
  fMe[0][1] = -c1 * s4 * s5 - s1 * c4 * s5;
  fMe[0][2] = c1 * c4 - s1 * s4;
  fMe[0][3] = c1 * this->_a1 - s1 * (this->_d3);

  fMe[1][0] = s1 * s4 * c5 - c1 * c4 * c5;
  fMe[1][1] = -s1 * s4 * s5 + c1 * c4 * s5;
  fMe[1][2] = s1 * c4 + c1 * s4;
  fMe[1][3] = s1 * this->_a1 + c1 * (this->_d3);

  fMe[2][0] = -s5;
  fMe[2][1] = -c5;
  fMe[2][2] = 0.f;
  fMe[2][3] = this->_d4 + q2;

  fMe[3][0] = 0.f;
  fMe[3][1] = 0.f;
  fMe[3][2] = 0.f;
  fMe[3][3] = 1;

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

  return;
}

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

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

  cVe.buildFrom(cMe);

  return;
}

void vpAfma4::get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
{
  vpHomogeneousMatrix fMc, cMf;
  get_fMc(q, fMc);
  cMf = fMc.inverse();

  cVf.buildFrom(cMf);

  return;
}

void vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
  double q4 = q[2]; // pan
  double q5 = q[3]; // tilt

  double c4 = cos(q4);
  double s4 = sin(q4);
  double c5 = cos(q5);
  double s5 = sin(q5);

  eJe.resize(6, 4);

  eJe = 0;

  eJe[0][0] = -(this->_a1 * c4 + this->_d3 * s4) * c5;
  eJe[0][1] = -s5;
  eJe[1][0] = (this->_a1 * c4 + this->_d3 * s4) * s5;
  eJe[1][1] = -c5;
  eJe[2][0] = (this->_a1 * s4 - this->_d3 * c4);
  eJe[3][0] = eJe[3][2] = -s5;
  eJe[4][0] = eJe[4][2] = -c5;
  eJe[5][3] = 1.;
}

void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
  fJe.resize(6, 4);

  double q1 = q[0]; // rot touret
  double q4 = q[2]; // pan

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c14 = cos(q1 + q4);
  double s14 = sin(q1 + q4);

  fJe = 0;

  fJe[0][0] = -s1 * this->_a1 - c1 * this->_d3;

  fJe[1][0] = c1 * this->_a1 - s1 * this->_d3;

  fJe[2][1] = 1.0;

  fJe[3][3] = c14;

  fJe[4][3] = s14;

  fJe[5][0] = fJe[5][2] = 1.0;
}

void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
{
  fJe_inverse.resize(4, 6);
  fJe_inverse = 0;

  double q1 = q[0]; // rot touret
  double q4 = q[2]; // pan

  double c1 = cos(q1);
  double s1 = sin(q1);
  double c14 = cos(q1 + q4);
  double s14 = sin(q1 + q4);

  double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;

  fJe_inverse[0][0] = (-s1 * this->_a1 - c1 * this->_d3) / det;
  fJe_inverse[0][1] = (c1 * this->_a1 - s1 * this->_d3) / det;

  fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;

  fJe_inverse[2][0] = -fJe_inverse[0][0];
  fJe_inverse[2][1] = -fJe_inverse[0][1];

  fJe_inverse[3][3] = c14;
  fJe_inverse[3][4] = s14;
}

vpColVector vpAfma4::getJointMin() const
{
  vpColVector qmin(4);
  for (unsigned int i = 0; i < 4; i++)
    qmin[i] = this->_joint_min[i];
  return qmin;
}

vpColVector vpAfma4::getJointMax() const
{
  vpColVector qmax(4);
  for (unsigned int i = 0; i < 4; i++)
    qmax[i] = this->_joint_max[i];
  return qmax;
}

VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
{
  vpRotationMatrix eRc;
  afma4._eMc.extract(eRc);
  vpRxyzVector rxyz(eRc);

  os << "Joint Max:" << std::endl
     << "\t" << afma4._joint_max[0] << "\t" << afma4._joint_max[1] << "\t" << afma4._joint_max[2] << "\t"
     << afma4._joint_max[3] << "\t" << std::endl

     << "Joint Min: " << std::endl
     << "\t" << afma4._joint_min[0] << "\t" << afma4._joint_min[1] << "\t" << afma4._joint_min[2] << "\t"
     << afma4._joint_min[3] << "\t" << std::endl

     << "a1: " << std::endl
     << "\t" << afma4._a1 << "\t" << std::endl

     << "d3: " << std::endl
     << "\t" << afma4._d3 << "\t" << std::endl

     << "d4: " << std::endl
     << "\t" << afma4._d4 << "\t" << std::endl

     << "eMc: " << std::endl
     << "\tTranslation (m): " << afma4._eMc[0][3] << " " << afma4._eMc[1][3] << " " << afma4._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;
}