vpViper650.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://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 ADEPT Viper 650 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/robot/vpViper650.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpXmlParserCamera.h>

static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ","eMc_TRANS_XYZ", NULL};

#ifdef VISP_HAVE_VIPER650_DATA
const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_schunk_gripper_without_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf");

const std::string vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf");

const std::string vpViper650::CONST_CAMERA_FILENAME =
    std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml");

#endif // VISP_HAVE_VIPER650_DATA

const char * const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
const char * const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
const char * const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
const char * const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";

const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA;



vpViper650::vpViper650()
  : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
{
  // Denavit Hartenberg parameters
  a1 = 0.075;
  a2 = 0.270;
  a3 = 0.090;
  d1 = 0.335;
  d4 = 0.295;
  d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
  c56 = -341.33 / 9102.22;

  // Software joint limits in radians
  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[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);

  init(); // Set the default tool
}

void
vpViper650::init (void)
{
  this->init(vpViper650::defaultTool);
  return;
}

void
vpViper650::init (const std::string &camera_extrinsic_parameters)
{
  //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
  this->parseConfigFile (camera_extrinsic_parameters);

  return ;
}

void
vpViper650::init (vpViper650::vpToolType tool,
                  vpCameraParameters::vpCameraParametersProjType proj_model)
{
  
  this->projModel = proj_model;
  
#ifdef VISP_HAVE_VIPER650_DATA
  // Read the robot parameters from files
  std::string filename_eMc;
  switch (tool) {
  case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
        filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
        filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
      break;
    }
    break;
  }
  case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
        filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
        filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
      break;
    }
    break;
  }
  case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
        filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
        filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
      break;
    }
    break;
  }
  case vpViper650::TOOL_GENERIC_CAMERA: {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
        filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
        filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
      break;
    }
    break;
  }
  case vpViper650::TOOL_CUSTOM: {
    throw vpRobotException (vpRobotException::badValue,
                            "No predefined file available for a custom tool"
                            "You should use init(vpViper650::vpToolType, const std::string&) or"
                            "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
  }
  default: {
    vpERROR_TRACE ("This error should not occur!");
    //       vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
    //         "que les specs de la classe ont ete modifiee, "
    //         "et que le code n'a pas ete mis a jour "
    //         "correctement.");
    //       vpERROR_TRACE ("Verifiez les valeurs possibles du type "
    //         "vpViper650::vpViper650ToolType, et controlez que "
    //         "tous les cas ont ete pris en compte dans la "
    //         "fonction init(camera).");
    break;
  }
  }
  
  this->init (filename_eMc);

#else // VISP_HAVE_VIPER650_DATA

  // Use here default values of the robot constant parameters.
  switch (tool) {
  case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
      erc[0] = vpMath::rad(0.07); // rx
      erc[1] = vpMath::rad(2.76); // ry
      erc[2] = vpMath::rad(-91.50); // rz
      etc[0] = -0.0453; // tx
      etc[1] =  0.0005; // ty
      etc[2] =  0.0728; // tz
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
      erc[0] = vpMath::rad(0.26); // rx
      erc[1] = vpMath::rad(2.12); // ry
      erc[2] = vpMath::rad(-91.31); // rz
      etc[0] = -0.0444; // tx
      etc[1] = -0.0005; // ty
      etc[2] =  0.1022; // tz
      break;
    }
    break;
  }
  case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
  case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA:  {
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
      erc[0] = vpMath::rad(0.15); // rx
      erc[1] = vpMath::rad(1.28); // ry
      erc[2] = vpMath::rad(-90.8); // rz
      etc[0] = -0.0456; // tx
      etc[1] = -0.0013; // ty
      etc[2] =  0.001; // tz
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
      erc[0] = vpMath::rad(0.72); // rx
      erc[1] = vpMath::rad(2.10); // ry
      erc[2] = vpMath::rad(-90.5); // rz
      etc[0] = -0.0444; // tx
      etc[1] = -0.0012; // ty
      etc[2] =  0.078; // tz
      break;
    }
    break;
  }
  case vpViper650::TOOL_GENERIC_CAMERA: {
    // Set eMc to identity
    switch(projModel) {
    case vpCameraParameters::perspectiveProjWithoutDistortion :
    case vpCameraParameters::perspectiveProjWithDistortion :
      erc[0] = 0; // rx
      erc[1] = 0; // ry
      erc[2] = 0; // rz
      etc[0] = 0; // tx
      etc[1] = 0; // ty
      etc[2] = 0; // tz
      break;
    }
    break;
  }
  case vpViper650::TOOL_CUSTOM: {
    throw vpRobotException (vpRobotException::badValue,
                            "No predefined parameters available for a custom tool"
                            "You should use init(vpViper650::vpToolType, const std::string&) or"
                            "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
  }
  }
  vpRotationMatrix eRc(erc);
  this->eMc.buildFrom(etc, eRc);
#endif // VISP_HAVE_VIPER650_DATA

  setToolType(tool);
  return ;
}

void
vpViper650::init(vpViper650::vpToolType tool,
                 const std::string &filename)
{
    this->setToolType(tool);
    this->parseConfigFile(filename.c_str());
}

void
vpViper650::init(vpViper650::vpToolType tool,
                 const vpHomogeneousMatrix &eMc_)
{
    this->setToolType(tool);
    this->set_eMc(eMc_);
}

void
vpViper650::parseConfigFile (const std::string &filename)
{
  size_t            dim;
  int               code;
  char              Ligne[FILENAME_MAX];
  char              namoption[100];
  FILE *            fdtask;
  int               numLn = 0;
  vpRxyzVector rot_eMc; // rotation
  vpTranslationVector trans_eMc; // translation
  bool get_rot_eMc = false;
  bool get_trans_eMc = false;

  //vpTRACE("Read the config file for constant parameters %s.", filename.c_str());
  if ((fdtask = fopen(filename.c_str(), "r" )) == NULL) {
    throw vpRobotException (vpRobotException::readingParametersError,
                            "Impossible to read the config file %s.", filename.c_str());
  }

  while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
    numLn ++;
    if ('#' == Ligne[0]) { continue; }
    {
      std::stringstream ss(Ligne);
      ss >> namoption;

      if (ss.fail()) {
        fclose (fdtask);
        throw(vpException(vpException::badValue,
                          "Cannot parse configuration file %s to retrieve option name"));
      }
    }

    dim = strlen(namoption);

    for (code = 0; NULL != opt_viper650[code]; ++ code)  {
      if (strncmp(opt_viper650[code], namoption, dim) == 0) {
        break;
      }
    }

    switch(code) {
    case 0:
      break; // Nothing to do: camera name

    case 1: {
      std::stringstream ss(Ligne);
      ss >> namoption >> rot_eMc[0] >> rot_eMc[1] >> rot_eMc[2];
      if (ss.fail()) {
        fclose (fdtask);
        throw(vpException(vpException::badValue,
                          "Cannot parse configuration file %s to retrieve rotation", filename.c_str()));
      }

      // Convert rotation from degrees to radians
      rot_eMc[0] *= M_PI / 180.0;
      rot_eMc[1] *= M_PI / 180.0;
      rot_eMc[2] *= M_PI / 180.0;
      get_rot_eMc = true;
      break;
    }

    case 2: {
      std::stringstream ss(Ligne);
      ss >> namoption >> trans_eMc[0] >> trans_eMc[1] >> trans_eMc[2];
      if (ss.fail()) {
        fclose (fdtask);
        throw(vpException(vpException::badValue,
                          "Cannot parse configuration file %s to retrieve translation", filename.c_str()));
      }
      get_trans_eMc = true;
      break;
    }

    default:
      vpERROR_TRACE ("Bad configuration file %s  "
                     "line #%d.", filename.c_str(), numLn);
    } /* SWITCH */
  } /* WHILE */

  fclose (fdtask);

  // Compute the eMc matrix from the translations and rotations
  if (get_rot_eMc && get_trans_eMc) {
    this->set_eMc(trans_eMc,rot_eMc);
  }
  else {
    throw vpRobotException (vpRobotException::readingParametersError,
                            "Could not read translation and rotation parameters from config file %s", filename.c_str());
  }

  return;
}


void
vpViper650::getCameraParameters (vpCameraParameters &cam,
                                 const unsigned int &image_width,
                                 const unsigned int &image_height) const
{
#if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_VIPER650_DATA)
  vpXmlParserCamera parser;
  switch (getToolType()) {
  case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
    std::cout << "Get camera parameters for camera \""
              << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl
              << "from the XML file: \""
              << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
    if (parser.parse(cam,
                     vpViper650::CONST_CAMERA_FILENAME,
                     vpViper650::CONST_MARLIN_F033C_CAMERA_NAME,
                     projModel,
                     image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
    std::cout << "Get camera parameters for camera \""
              << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
              << "from the XML file: \""
              << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
    if (parser.parse(cam,
                     vpViper650::CONST_CAMERA_FILENAME,
                     vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME,
                     projModel,
                     image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
    std::cout << "Get camera parameters for camera \""
              << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
              << "from the XML file: \""
              << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
    if (parser.parse(cam,
                     vpViper650::CONST_CAMERA_FILENAME,
                     vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
                     projModel,
                     image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_GENERIC_CAMERA: {
    std::cout << "Get camera parameters for camera \""
              << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
              << "from the XML file: \""
              << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
    if (parser.parse(cam,
                     vpViper650::CONST_CAMERA_FILENAME,
                     vpViper650::CONST_GENERIC_CAMERA_NAME,
                     projModel,
                     image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_CUSTOM: {
    throw vpRobotException (vpRobotException::badValue,
                            "No intrinsic parameters available for a custom tool");
  }
  default: {
    vpERROR_TRACE ("This error should not occur!");
    //       vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
    //        "que les specs de la classe ont ete modifiee, "
    //        "et que le code n'a pas ete mis a jour "
    //        "correctement.");
    //       vpERROR_TRACE ("Verifiez les valeurs possibles du type "
    //        "vpViper650::vpViper650ToolType, et controlez que "
    //        "tous les cas ont ete pris en compte dans la "
    //        "fonction init(camera).");
    throw vpRobotException (vpRobotException::readingParametersError,
                            "Impossible to read the camera parameters.");
  }
  }
#else
  // Set default parameters
  switch (getToolType()) {
  case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
    // Set default intrinsic camera parameters for 640x480 images
    if (image_width == 640 && image_height == 480) {
      std::cout << "Get default camera parameters for camera \""
                << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
      switch(this->projModel) {
      case vpCameraParameters::perspectiveProjWithoutDistortion :
        cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
        break;
      case vpCameraParameters::perspectiveProjWithDistortion :
        cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
        break;
      }
    }
    else {
      vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
  case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
    // Set default intrinsic camera parameters for 640x480 images
    if (image_width == 640 && image_height == 480) {
      std::cout << "Get default camera parameters for camera \""
                << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
      switch(this->projModel) {
      case vpCameraParameters::perspectiveProjWithoutDistortion :
        cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
        break;
      case vpCameraParameters::perspectiveProjWithDistortion :
        cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
        break;
      }
    }
    else {
      vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_GENERIC_CAMERA: {
    // Set default intrinsic camera parameters for 640x480 images
    if (image_width == 640 && image_height == 480) {
      std::cout << "Get default camera parameters for camera \""
                << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
      switch(this->projModel) {
      case vpCameraParameters::perspectiveProjWithoutDistortion :
        cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
        break;
      case vpCameraParameters::perspectiveProjWithDistortion :
        cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
        break;
      }
    }
    else {
      vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
      throw vpRobotException (vpRobotException::readingParametersError,
                              "Impossible to read the camera parameters.");
    }
    break;
  }
  case vpViper650::TOOL_CUSTOM: {
    throw vpRobotException (vpRobotException::badValue,
                            "No intrinsic parameters available for a custom tool");
  }
  default:
    vpERROR_TRACE ("This error should not occur!");
    throw vpRobotException (vpRobotException::readingParametersError,
                            "Impossible to read the camera parameters.");
    break;
  }
#endif
  return;
}

void
vpViper650::getCameraParameters (vpCameraParameters &cam,
                                 const vpImage<unsigned char> &I) const
{
  getCameraParameters(cam,I.getWidth(),I.getHeight());
}
void
vpViper650::getCameraParameters (vpCameraParameters &cam,
                                 const vpImage<vpRGBa> &I) const
{
  getCameraParameters(cam,I.getWidth(),I.getHeight());
}