Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpRobotWireFrameSimulator.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Basic class used to make robot simulators.
32  */
33 
34 #ifndef vpRobotWireFrameSimulator_HH
35 #define vpRobotWireFrameSimulator_HH
36 
42 #include <visp3/core/vpConfig.h>
43 
44 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
45 
46 #include <cmath> // std::fabs
47 #include <limits> // numeric_limits
48 
49 #include <thread>
50 #include <mutex>
51 
52 #include <visp3/gui/vpDisplayD3D.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayGTK.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/robot/vpRobot.h>
58 #include <visp3/robot/vpRobotSimulator.h>
59 #include <visp3/robot/vpWireFrameSimulator.h>
60 
79 {
80 public:
82 
83  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
84 
85 protected:
87  double tcur;
89  double tprev;
90 
92  Bound_scene *robotArms;
93 
95  unsigned int size_fMi;
103 
110 
111  std::thread *m_thread;
112 
113  std::mutex m_mutex_fMi;
114  std::mutex m_mutex_eMc;
115  std::mutex m_mutex_artVel;
116  std::mutex m_mutex_artCoord;
117  std::mutex m_mutex_velocity;
118  std::mutex m_mutex_display;
119  std::mutex m_mutex_robotStop;
120  std::mutex m_mutex_frame;
122  std::mutex m_mutex_scene;
123 
125 
127  bool robotStop;
131  unsigned int jointLimitArt;
134 
137 
138 #if defined(VISP_HAVE_X11)
140 #elif defined(VISP_HAVE_GDI)
141  vpDisplayGDI display;
142 #elif defined(HAVE_OPENCV_HIGHGUI)
143  vpDisplayOpenCV display;
144 #elif defined(VISP_HAVE_D3D9)
145  vpDisplayD3D display;
146 #elif defined(VISP_HAVE_GTK)
147  vpDisplayGTK display;
148 #endif
149 
151 
158 
162 
163  bool verbose_;
164 
165 public:
167  explicit vpRobotWireFrameSimulator(bool display);
168 
177  {
178  // if(px_ext != 1 && py_ext != 1)
179  // we assume px_ext and py_ext > 0
180  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
181  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
182  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
183  else {
184  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
185  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
186  }
187  }
196  {
198  }
199 
200  void getInternalView(vpImage<vpRGBa> &I);
201  void getInternalView(vpImage<unsigned char> &I);
202 
209  vpHomogeneousMatrix get_fMo() const { return fMo; }
210 
211  /* Display functions */
212  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
213  void initScene(const char *obj, const char *desiredObject);
214  void initScene(const vpSceneObject &obj);
215  void initScene(const char *obj);
216 
222  void setCameraColor(const vpColor &col) { camColor = col; }
223 
233  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
234  {
235  constantSamplingTimeMode = _constantSamplingTimeMode;
236  }
237 
244  void setCurrentViewColor(const vpColor &col) { curColor = col; }
245 
252  void setDesiredViewColor(const vpColor &col) { desColor = col; }
253 
260  {
262  }
263 
271  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
279  {
281  }
285  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
286 
296  inline void setSamplingTime(const double &delta_t)
297  {
298  if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
299  this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
300  }
301  else {
302  this->delta_t_ = delta_t;
303  }
304  }
308  void setSingularityManagement(bool sm) { singularityManagement = sm; }
309 
313  void setVerbose(bool verbose) { this->verbose_ = verbose; }
314 
320  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
322 
323 protected:
330  static void launcher(vpRobotWireFrameSimulator &simulator)
331  {
332  simulator.updateArticularPosition();
333  }
334 
339  virtual void updateArticularPosition() = 0;
341  virtual int isInJointLimit() = 0;
344  virtual void computeArticularVelocity() = 0;
345 
346  /* Display functions */
347  void initDisplay() { ; }
348  virtual void initArms() = 0;
349 
351  {
352  m_mutex_artCoord.lock();
353  vpColVector artCoordTmp(6);
354  artCoordTmp = artCoord;
355  m_mutex_artCoord.unlock();
356  return artCoordTmp;
357  }
358  void set_artCoord(const vpColVector &coord)
359  {
360  m_mutex_artCoord.lock();
361  artCoord = coord;
362  m_mutex_artCoord.unlock();
363  }
364 
366  {
367  m_mutex_artVel.lock();
368  vpColVector artVelTmp(artVel);
369  m_mutex_artVel.unlock();
370  return artVelTmp;
371  }
372  void set_artVel(const vpColVector &vel)
373  {
374  m_mutex_artVel.lock();
375  artVel = vel;
376  m_mutex_artVel.unlock();
377  }
378 
380  {
381  m_mutex_velocity.lock();
382  vpColVector velocityTmp = velocity;
383  m_mutex_velocity.unlock();
384  return velocityTmp;
385  }
386  void set_velocity(const vpColVector &vel)
387  {
388  m_mutex_velocity.lock();
389  velocity = vel;
390  m_mutex_velocity.unlock();
391  }
392 
393  void set_displayBusy(const bool &status)
394  {
395  m_mutex_display.lock();
396  displayBusy = status;
397  m_mutex_display.unlock();
398  }
400  {
401  m_mutex_display.lock();
402  bool status = displayBusy;
403  if (!displayBusy)
404  displayBusy = true;
405  m_mutex_display.unlock();
406  return status;
407  }
408 
411  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
413 };
414 
415 #endif
416 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:101
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
This class aims to be a basis used to create all the robot simulators.
This class aims to be a basis used to create all the simulators of robots.
virtual void computeArticularVelocity()=0
void setDisplayRobotType(const vpDisplayRobotType dispType)
virtual int isInJointLimit()=0
void setGraphicsThickness(unsigned int thickness)
void set_velocity(const vpColVector &vel)
void setCurrentViewColor(const vpColor &col)
static void launcher(vpRobotWireFrameSimulator &simulator)
void set_displayBusy(const bool &status)
void setDesiredViewColor(const vpColor &col)
vpHomogeneousMatrix getExternalCameraPosition() const
vpHomogeneousMatrix get_fMo() const
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
void set_artCoord(const vpColVector &coord)
void setCameraColor(const vpColor &col)
virtual void initArms()=0
vpCameraParameters getExternalCameraParameters() const
virtual void updateArticularPosition()=0
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
virtual void get_fMi(vpHomogeneousMatrix *fMit)=0
void set_fMo(const vpHomogeneousMatrix &fMo_)
void setSamplingTime(const double &delta_t)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
vpHomogeneousMatrix get_cMo() const
vpHomogeneousMatrix fMo
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
VISP_EXPORT double getMinTimeForUsleepCall()