vpRobotWireFrameSimulator.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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:
 * Basic class used to make robot simulators.
 *
*****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
#include <visp3/robot/vpRobotWireFrameSimulator.h>
#include <visp3/robot/vpSimulatorViper850.h>
 
#include "../wireframe-simulator/vpBound.h"
#include "../wireframe-simulator/vpScene.h"
#include "../wireframe-simulator/vpVwstack.h"
 
vpRobotWireFrameSimulator::vpRobotWireFrameSimulator()
  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(nullptr), size_fMi(8), fMi(nullptr),
  artCoord(), artVel(), velocity(), m_thread(), m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(),
  m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(),
  m_mutex_scene(), displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false),
  singularityManagement(true), cameraParam(),
#if defined(VISP_HAVE_DISPLAY)
  display(),
#endif
  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
  verbose_(false)
{
  setSamplingTime(0.010);
  velocity.resize(6);
  I.resize(480, 640);
  I = 255;
#if defined(VISP_HAVE_DISPLAY)
  display.init(I, 0, 0, "The External view");
#endif
}
 
vpRobotWireFrameSimulator::vpRobotWireFrameSimulator(bool do_display)
  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(nullptr), size_fMi(8), fMi(nullptr),
  artCoord(), artVel(), velocity(), m_thread(), m_mutex_fMi(), m_mutex_eMc(), m_mutex_artVel(), m_mutex_artCoord(),
  m_mutex_velocity(), m_mutex_display(), m_mutex_robotStop(), m_mutex_frame(), m_mutex_setVelocityCalled(), m_mutex_scene(),
  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
  cameraParam(),
#if defined(VISP_HAVE_DISPLAY)
  display(),
#endif
  displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false), setVelocityCalled(false),
  verbose_(false)
{
  setSamplingTime(0.010);
  velocity.resize(6);
  I.resize(480, 640);
  I = 255;
 
#if defined(VISP_HAVE_DISPLAY)
  if (do_display)
    this->display.init(I, 0, 0, "The External view");
#endif
}
 
void vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
{
  m_mutex_scene.lock();
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj, desired_object);
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  displayCamera = false;
  m_mutex_scene.unlock();
}
 
void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
{
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj, desired_object);
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  displayCamera = false;
}
 
void vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
{
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj);
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  displayCamera = false;
}
 
void vpRobotWireFrameSimulator::initScene(const char *obj)
{
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj);
  if (displayCamera) {
    free_Bound_scene(&(this->camera));
  }
  displayCamera = false;
}
 
void vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
{
 
  if (!sceneInitialized)
    throw;
 
  double u;
  double v;
  // if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I_.getWidth() / (2 * px_int);
    v = (double)I_.getHeight() / (2 * py_int);
  }
  else {
    u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
    v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
  }
 
  float o44c[4][4], o44cd[4][4], x, y, z;
  Matrix id = IDENTITY_MATRIX;
 
  vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
  this->cMo = rotz * cMo;
 
  vp2jlc_matrix(cMo.inverse(), o44c);
  vp2jlc_matrix(cdMo.inverse(), o44cd);
 
  while (get_displayBusy())
    vpTime::wait(2);
 
  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayObject)
    display_scene(id, this->scene, I_, curColor);
 
  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayDesiredObject) {
    if (desiredObject == D_TOOL)
      display_scene(o44cd, desiredScene, I_, vpColor::red);
    else
      display_scene(id, desiredScene, I_, desColor);
  }
  delete[] fMit;
  set_displayBusy(false);
}
 
void vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
{
 
  if (!sceneInitialized)
    throw;
 
  double u;
  double v;
  // if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_int);
    v = (double)I.getHeight() / (2 * py_int);
  }
  else {
    u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
    v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
  }
 
  float o44c[4][4], o44cd[4][4], x, y, z;
  Matrix id = IDENTITY_MATRIX;
 
  vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
  this->cMo = rotz * cMo;
 
  vp2jlc_matrix(cMo.inverse(), o44c);
  vp2jlc_matrix(cdMo.inverse(), o44cd);
 
  while (get_displayBusy())
    vpTime::wait(2);
 
  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayObject) {
    display_scene(id, this->scene, I_, curColor);
  }
 
  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayDesiredObject) {
    if (desiredObject == D_TOOL)
      display_scene(o44cd, desiredScene, I_, vpColor::red);
    else
      display_scene(id, desiredScene, I_, desColor);
  }
  delete[] fMit;
  set_displayBusy(false);
}
 
vpHomogeneousMatrix vpRobotWireFrameSimulator::get_cMo()
{
  vpHomogeneousMatrix cMoTemp;
  vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
  delete[] fMit;
  return cMoTemp;
}
 
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning:
// libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
void dummy_vpRobotWireFrameSimulator() { };
#endif