vpAfma4.cpp

/****************************************************************************
 *
 * $Id: vpAfma4.cpp 4632 2014-02-03 17:06:40Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpDebug.h>
#include <visp/vpVelocityTwistMatrix.h>
#include <visp/vpRobotException.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpRxyzVector.h>
#include <visp/vpTranslationVector.h>
#include <visp/vpRotationMatrix.h>
#include <visp/vpAfma4.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 on the NAS
  // at Inria 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;
}