Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
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(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
45 
46 #include <cmath> // std::fabs
47 #include <limits> // numeric_limits
48 #if defined(_WIN32)
49 // Include WinSock2.h before windows.h to ensure that winsock.h is not
50 // included by windows.h since winsock.h and winsock2.h are incompatible
51 #include <WinSock2.h>
52 #include <windows.h>
53 #elif defined(VISP_HAVE_PTHREAD)
54 #include <pthread.h>
55 #endif
56 
57 #include <visp3/core/vpMutex.h>
58 #include <visp3/gui/vpDisplayD3D.h>
59 #include <visp3/gui/vpDisplayGDI.h>
60 #include <visp3/gui/vpDisplayGTK.h>
61 #include <visp3/gui/vpDisplayOpenCV.h>
62 #include <visp3/gui/vpDisplayX.h>
63 #include <visp3/robot/vpRobot.h>
64 #include <visp3/robot/vpRobotSimulator.h>
65 #include <visp3/robot/vpWireFrameSimulator.h>
66 
85 {
86 public:
88 
89  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
90 
91 protected:
93  double tcur;
95  double tprev;
96 
98  Bound_scene *robotArms;
99 
101  unsigned int size_fMi;
109 
116 
117 #if defined(_WIN32)
118  HANDLE hThread;
119 #elif defined(VISP_HAVE_PTHREAD)
120  pthread_t thread;
121  pthread_attr_t attr;
122 #endif
123 
134 
136 
138  bool robotStop;
142  unsigned int jointLimitArt;
145 
148 
149 #if defined(VISP_HAVE_X11)
151 #elif defined(VISP_HAVE_GDI)
152  vpDisplayGDI display;
153 #elif defined(HAVE_OPENCV_HIGHGUI)
154  vpDisplayOpenCV display;
155 #elif defined(VISP_HAVE_D3D9)
156  vpDisplayD3D display;
157 #elif defined(VISP_HAVE_GTK)
158  vpDisplayGTK display;
159 #endif
160 
162 
169 
173 
174  bool verbose_;
175 
176 public:
178  explicit vpRobotWireFrameSimulator(bool display);
179 
188  {
189  // if(px_ext != 1 && py_ext != 1)
190  // we assume px_ext and py_ext > 0
191  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
192  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
193  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
194  else {
195  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
196  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
197  }
198  }
207  {
209  }
210 
211  void getInternalView(vpImage<vpRGBa> &I);
212  void getInternalView(vpImage<unsigned char> &I);
213 
220  vpHomogeneousMatrix get_fMo() const { return fMo; }
221 
222  /* Display functions */
223  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
224  void initScene(const char *obj, const char *desiredObject);
225  void initScene(const vpSceneObject &obj);
226  void initScene(const char *obj);
227 
233  void setCameraColor(const vpColor &col) { camColor = col; }
234 
244  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
245  {
246  constantSamplingTimeMode = _constantSamplingTimeMode;
247  }
248 
255  void setCurrentViewColor(const vpColor &col) { curColor = col; }
256 
263  void setDesiredViewColor(const vpColor &col) { desColor = col; }
264 
271  {
273  }
274 
282  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
290  {
292  }
296  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
297 
307  inline void setSamplingTime(const double &delta_t)
308  {
309  if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
310  this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
311  }
312  else {
313  this->delta_t_ = delta_t;
314  }
315  }
319  void setSingularityManagement(bool sm) { singularityManagement = sm; }
320 
324  void setVerbose(bool verbose) { this->verbose_ = verbose; }
325 
331  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
333 
334 protected:
341 #if defined(_WIN32)
342  static DWORD WINAPI launcher(LPVOID lpParam)
343  {
344  (static_cast<vpRobotWireFrameSimulator *>(lpParam))->updateArticularPosition();
345  return 0;
346  }
347 #elif defined(VISP_HAVE_PTHREAD)
348  static void *launcher(void *arg)
349  {
350  (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
351  // pthread_exit((void*) 0);
352  return nullptr;
353  }
354 #endif
355 
358  virtual void updateArticularPosition() = 0;
360  virtual int isInJointLimit() = 0;
363  virtual void computeArticularVelocity() = 0;
364 
365  /* Display functions */
366  void initDisplay() { ; }
367  virtual void initArms() = 0;
368 
370  {
371  m_mutex_artCoord.lock();
372  vpColVector artCoordTmp(6);
373  artCoordTmp = artCoord;
374  m_mutex_artCoord.unlock();
375  return artCoordTmp;
376  }
377  void set_artCoord(const vpColVector &coord)
378  {
379  m_mutex_artCoord.lock();
380  artCoord = coord;
381  m_mutex_artCoord.unlock();
382  }
383 
385  {
386  m_mutex_artVel.lock();
387  vpColVector artVelTmp(artVel);
388  m_mutex_artVel.unlock();
389  return artVelTmp;
390  }
391  void set_artVel(const vpColVector &vel)
392  {
393  m_mutex_artVel.lock();
394  artVel = vel;
395  m_mutex_artVel.unlock();
396  }
397 
399  {
400  m_mutex_velocity.lock();
401  vpColVector velocityTmp = velocity;
402  m_mutex_velocity.unlock();
403  return velocityTmp;
404  }
405  void set_velocity(const vpColVector &vel)
406  {
407  m_mutex_velocity.lock();
408  velocity = vel;
409  m_mutex_velocity.unlock();
410  }
411 
412  void set_displayBusy(const bool &status)
413  {
414  m_mutex_display.lock();
415  displayBusy = status;
416  m_mutex_display.unlock();
417  }
419  {
420  m_mutex_display.lock();
421  bool status = displayBusy;
422  if (!displayBusy)
423  displayBusy = true;
424  m_mutex_display.unlock();
425  return status;
426  }
427 
430  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
432 };
433 
434 #endif
435 #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:240
unsigned int getHeight() const
Definition: vpImage.h:182
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
void unlock()
Definition: vpMutex.h:106
void lock()
Definition: vpMutex.h:90
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)
void set_displayBusy(const bool &status)
void setDesiredViewColor(const vpColor &col)
vpHomogeneousMatrix getExternalCameraPosition() const
vpHomogeneousMatrix get_fMo() const
static void * launcher(void *arg)
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()