vpBiclops.cpp

/****************************************************************************
 *
 * $Id: vpBiclops.cpp 3698 2012-05-03 15:20:05Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 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 Biclops robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

/* ----------------------------------------------------------------------- */
/* --- INCLUDE ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */



#include <visp/vpBiclops.h>
#include <visp/vpDebug.h>
#include <visp/vpRobotException.h>
#include <visp/vpMath.h>
#include <math.h>


/* ------------------------------------------------------------------------ */
/* --- COMPUTE ------------------------------------------------------------ */
/* ------------------------------------------------------------------------ */
const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
const float vpBiclops::h    = 0.048f; /*<! Vertical offset from last joint to camera frame. */
const float vpBiclops::panJointLimit  = (float)(M_PI/2.0); 
const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5); 
const float vpBiclops::speedLimit = (float)(M_PI/3.0); 
void
vpBiclops::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc)
{
  vpHomogeneousMatrix fMe = get_fMe(q);
  fMc = fMe * cMe_.inverse();

  vpCDEBUG (6) << "camera position: " << std::endl << fMc;

  return ;
}

void
vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc)
{
  vpHomogeneousMatrix fMe = get_fMe(q);
  fMc = fMe * cMe_.inverse();

  vpCDEBUG (6) << "camera position: " << std::endl << fMc;

  return ;
}

vpHomogeneousMatrix
vpBiclops::computeMGD (const vpColVector & q)
{
  vpHomogeneousMatrix fMc;

  computeMGD (q, fMc);

  return fMc;
}

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

  get_fMc (q, fMc);

  return fMc;
}

vpHomogeneousMatrix
vpBiclops::get_fMe (const vpColVector & q)
{
  vpHomogeneousMatrix fMe;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  double            q1 = q[0]; // pan
  double            q2 = q[1]; // tilt

  double            c1 = cos(q1);
  double            s1 = sin(q1);
  double            c2 = cos(q2);
  double            s2 = sin(q2);

  if (dh_model_ == DH1)
  {
    fMe[0][0] = -c1*s2;
    fMe[0][1] = -s1;
    fMe[0][2] = c1*c2;
    fMe[0][3] = 0;

    fMe[1][0] = -s1*s2;
    fMe[1][1] = c1;
    fMe[1][2] = s1*c2;
    fMe[1][3] = 0;

    fMe[2][0] = -c2;
    fMe[2][1] = 0;
    fMe[2][2] = -s2;
    fMe[2][3] = 0;

    fMe[3][0] = 0;
    fMe[3][1] = 0;
    fMe[3][2] = 0;
    fMe[3][3] = 1;
  }
  else
  {
    fMe[0][0] = c1*s2;
    fMe[0][1] = -s1;
    fMe[0][2] = c1*c2;
    fMe[0][3] = 0;

    fMe[1][0] = s1*s2;
    fMe[1][1] = c1;
    fMe[1][2] = s1*c2;
    fMe[1][3] = 0;

    fMe[2][0] = -c2;
    fMe[2][1] = 0;
    fMe[2][2] = s2;
    fMe[2][3] = 0;

    fMe[3][0] = 0;
    fMe[3][1] = 0;
    fMe[3][2] = 0;
    fMe[3][3] = 1;
  }

  return fMe;
}

void
vpBiclops::computeMGD (const vpColVector &q, vpPoseVector &fvc)
{
  vpHomogeneousMatrix fMc;

  get_fMc (q, fMc);
  fvc.buildFrom(fMc.inverse());

  return ;
}

void
vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc)
{
  vpHomogeneousMatrix fMc;

  get_fMc (q, fMc);
  fvc.buildFrom(fMc.inverse());

  return ;
}


/* ---------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */

vpBiclops::vpBiclops (void)
{
  init();
}
/* ---------------------------------------------------------------------- */
/* --- PRIVATE ---------------------------------------------------------- */
/* ---------------------------------------------------------------------- */

void
vpBiclops::init ()
{
  set_cMe();
  return ;
}


/* ----------------------------------------------------------------------- */
/* --- DISPLAY ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */

std::ostream & operator << (std::ostream & os,
                const vpBiclops & /*constant*/)
{

  os
    << "Geometric parameters: " << std::endl
    << "h: "
    << "\t" << vpBiclops::h << std::endl;

  return os;
}


void
vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe)
{
  cVe.buildFrom(cMe_) ;
}

void
vpBiclops::set_cMe()
{
  vpHomogeneousMatrix eMc ;

  eMc[0][0] = 0;
  eMc[0][1] = -1;
  eMc[0][2] = 0;
  eMc[0][3] = h;

  eMc[1][0] = 1;
  eMc[1][1] = 0;
  eMc[1][2] = 0;
  eMc[1][3] = 0;

  eMc[2][0] = 0;
  eMc[2][1] = 0;
  eMc[2][2] = 1;
  eMc[2][3] = 0;

  eMc[3][0] = 0;
  eMc[3][1] = 0;
  eMc[3][2] = 0;
  eMc[3][3] = 1;

  cMe_ = eMc.inverse()  ;
}

void
vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe)
{


  eJe.resize(6,2) ;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  double s2 = sin(q[1]) ;
  double c2 = cos(q[1]) ;

  eJe = 0;

  if (dh_model_ == DH1)
  {
    eJe[3][0] = -c2;
    eJe[4][1] = 1;
    eJe[5][0] = -s2;
  }
  else
  {
    eJe[3][0] = -c2;
    eJe[4][1] = -1;
    eJe[5][0] = s2;

  }

}
void
vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe)
{

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  fJe.resize(6,2) ;

  double s1 = sin(q[0]) ;
  double c1 = cos(q[0]) ;

  fJe = 0;

  if (dh_model_ == DH1)
  {
    fJe[3][1] = -s1;
    fJe[4][1] = c1;
    fJe[5][0] = 1;
  }
  else
  {
    fJe[3][1] = s1;
    fJe[4][1] = -c1;
    fJe[5][0] = 1;
  }
}