vpViper650.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 ADEPT Viper 650 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/robot/vpViper650.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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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;
    case vpCameraParameters::ProjWithKannalaBrandtDistortion:
      throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
      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)
{
  vpRxyzVector erc_;        // eMc rotation
  vpTranslationVector etc_; // eMc translation

  std::ifstream fdconfig(filename.c_str(), std::ios::in);

  if (!fdconfig.is_open()) {
    throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
                           filename.c_str());
  }

  std::string line;
  int lineNum = 0;
  bool get_erc = false;
  bool get_etc = false;
  int code;

  while (std::getline(fdconfig, line)) {
    lineNum++;
    if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
      continue;
    }
    std::istringstream ss(line);
    std::string key;
    ss >> key;

    for (code = 0; NULL != opt_viper650[code]; ++code) {
      if (key.compare(opt_viper650[code]) == 0) {
        break;
      }
    }

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

    case 1: {
      ss >> erc_[0] >> erc_[1] >> erc_[2];

      // Convert rotation from degrees to radians
      erc_ = erc_ * M_PI / 180.0;
      get_erc = true;
      break;
    }

    case 2: {
      ss >> etc_[0] >> etc_[1] >> etc_[2];
      get_etc = true;
      break;
    }

    default:
      throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
                             filename.c_str(), lineNum));
    }
  }
  fdconfig.close();

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

void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
                                     const unsigned int &image_height) const
{
#if 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;
      case vpCameraParameters::ProjWithKannalaBrandtDistortion:
        throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
        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;
      case vpCameraParameters::ProjWithKannalaBrandtDistortion:
        throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
        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;
      case vpCameraParameters::ProjWithKannalaBrandtDistortion:
        throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
        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");
  }
  }
#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());
}