vpRobotWireFrameSimulator.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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.
 */

 
#ifndef VP_ROBOT_WIREFRAME_SIMULATOR_H
#define VP_ROBOT_WIREFRAME_SIMULATOR_H
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
 
#include <cmath>  // std::fabs
#include <limits> // numeric_limits
 
#include <thread>
#include <mutex>
 
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpRobotSimulator.h>
#include <visp3/robot/vpWireFrameSimulator.h>
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
{
public:
  vpImage<vpRGBa> I;
 
  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
 
protected:
  double tcur;
  double tprev;

  Bound_scene *robotArms;

  unsigned int size_fMi;
  vpHomogeneousMatrix *fMi;

  vpColVector artCoord;
  vpColVector artVel;
  vpColVector velocity;
 
  std::thread *m_thread;
 
  std::mutex m_mutex_fMi;
  std::mutex m_mutex_eMc;
  std::mutex m_mutex_artVel;
  std::mutex m_mutex_artCoord;
  std::mutex m_mutex_velocity;
  std::mutex m_mutex_display;
  std::mutex m_mutex_robotStop;
  std::mutex m_mutex_frame;
  std::mutex m_mutex_setVelocityCalled;
  std::mutex m_mutex_scene;
 
  bool displayBusy;

  bool robotStop;
  bool jointLimit;
  unsigned int jointLimitArt;
  bool singularityManagement;

  vpCameraParameters cameraParam;
 
#if defined(VISP_HAVE_X11)
  vpDisplayX display;
#elif defined(VISP_HAVE_GDI)
  vpDisplayGDI display;
#elif defined(HAVE_OPENCV_HIGHGUI)
  vpDisplayOpenCV display;
#elif defined(VISP_HAVE_D3D9)
  vpDisplayD3D display;
#elif defined(VISP_HAVE_GTK)
  vpDisplayGTK display;
#endif
 
  vpDisplayRobotType displayType;
 
  bool displayAllowed;
  bool constantSamplingTimeMode;

  bool setVelocityCalled;
 
public:
  vpRobotWireFrameSimulator();
  VP_EXPLICIT vpRobotWireFrameSimulator(bool display);


  vpCameraParameters getExternalCameraParameters() const
  {
    // if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }

  vpHomogeneousMatrix getExternalCameraPosition() const
  {
    return this->vpWireFrameSimulator::getExternalCameraPosition();
  }
 
  void getInternalView(vpImage<vpRGBa> &I);
  void getInternalView(vpImage<unsigned char> &I);
 
  vpHomogeneousMatrix get_cMo();
  vpHomogeneousMatrix get_fMo() const { return fMo; }
 
  /* Display functions */
  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
  void initScene(const char *obj, const char *desiredObject);
  void initScene(const vpSceneObject &obj);
  void initScene(const char *obj);

  void setCameraColor(const vpColor &col) { camColor = col; }

  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
  {
    constantSamplingTimeMode = _constantSamplingTimeMode;
  }

  void setCurrentViewColor(const vpColor &col) { curColor = col; }

  void setDesiredViewColor(const vpColor &col) { desColor = col; }

  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
  {
    this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
  }

  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
  void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
  {
    this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
  }

  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }

  inline void setSamplingTime(const double &delta_t) VP_OVERRIDE
  {
    if (delta_t < (vpTime::getMinTimeForUsleepCall() * 1e-3)) {
      this->delta_t_ = (vpTime::getMinTimeForUsleepCall() * 1e-3);
    }
    else {
      this->delta_t_ = delta_t;
    }
  }

  void setSingularityManagement(bool sm) { singularityManagement = sm; }

  void setVerbose(bool verbose) { this->verbose_ = verbose; }

  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
 
protected:

  static void launcher(vpRobotWireFrameSimulator &simulator)
  {
    simulator.updateArticularPosition();
  }

  virtual void updateArticularPosition() = 0;
  virtual int isInJointLimit() = 0;
  virtual void computeArticularVelocity() = 0;
 
  /* Display functions */
  void initDisplay() { }
  virtual void initArms() = 0;
 
  vpColVector get_artCoord()
  {
    m_mutex_artCoord.lock();
    vpColVector artCoordTmp(6);
    artCoordTmp = artCoord;
    m_mutex_artCoord.unlock();
    return artCoordTmp;
  }
  void set_artCoord(const vpColVector &coord)
  {
    m_mutex_artCoord.lock();
    artCoord = coord;
    m_mutex_artCoord.unlock();
  }
 
  vpColVector get_artVel()
  {
    m_mutex_artVel.lock();
    vpColVector artVelTmp(artVel);
    m_mutex_artVel.unlock();
    return artVelTmp;
  }
  void set_artVel(const vpColVector &vel)
  {
    m_mutex_artVel.lock();
    artVel = vel;
    m_mutex_artVel.unlock();
  }
 
  vpColVector get_velocity()
  {
    m_mutex_velocity.lock();
    vpColVector velocityTmp = velocity;
    m_mutex_velocity.unlock();
    return velocityTmp;
  }
  void set_velocity(const vpColVector &vel)
  {
    m_mutex_velocity.lock();
    velocity = vel;
    m_mutex_velocity.unlock();
  }
 
  void set_displayBusy(const bool &status)
  {
    m_mutex_display.lock();
    displayBusy = status;
    m_mutex_display.unlock();
  }
  bool get_displayBusy()
  {
    m_mutex_display.lock();
    bool status = displayBusy;
    if (!displayBusy)
      displayBusy = true;
    m_mutex_display.unlock();
    return status;
  }

  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
};
END_VISP_NAMESPACE
#endif
#endif