Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpRobotWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Basic class used to make robot simulators.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #ifndef vpRobotWireFrameSimulator_HH
40 #define vpRobotWireFrameSimulator_HH
41 
47 #include <visp3/core/vpConfig.h>
48 
49 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
50 
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 #if defined(_WIN32)
54 // Include WinSock2.h before windows.h to ensure that winsock.h is not
55 // included by windows.h since winsock.h and winsock2.h are incompatible
56 #include <WinSock2.h>
57 #include <windows.h>
58 #elif defined(VISP_HAVE_PTHREAD)
59 #include <pthread.h>
60 #endif
61 
62 #include <visp3/core/vpMutex.h>
63 #include <visp3/gui/vpDisplayD3D.h>
64 #include <visp3/gui/vpDisplayGDI.h>
65 #include <visp3/gui/vpDisplayGTK.h>
66 #include <visp3/gui/vpDisplayOpenCV.h>
67 #include <visp3/gui/vpDisplayX.h>
68 #include <visp3/robot/vpRobot.h>
69 #include <visp3/robot/vpRobotSimulator.h>
70 #include <visp3/robot/vpWireFrameSimulator.h>
71 
90 {
91 public:
93 
94  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
95 
96 protected:
98  double tcur;
100  double tprev;
101 
103  Bound_scene *robotArms;
104 
106  unsigned int size_fMi;
114 
121 
122 #if defined(_WIN32)
123  HANDLE hThread;
124 #elif defined(VISP_HAVE_PTHREAD)
125  pthread_t thread;
126  pthread_attr_t attr;
127 #endif
128 
139 
141 
143  bool robotStop;
147  unsigned int jointLimitArt;
150 
153 
154 #if defined VISP_HAVE_X11
156 #elif defined VISP_HAVE_GDI
157  vpDisplayGDI display;
158 #elif defined VISP_HAVE_OPENCV
159  vpDisplayOpenCV display;
160 #elif defined VISP_HAVE_D3D9
161  vpDisplayD3D display;
162 #elif defined VISP_HAVE_GTK
163  vpDisplayGTK display;
164 #endif
165 
167 
174 
178 
179  bool verbose_;
180 
181 public:
183  explicit vpRobotWireFrameSimulator(bool display);
184  virtual ~vpRobotWireFrameSimulator();
185 
194  {
195  // if(px_ext != 1 && py_ext != 1)
196  // we assume px_ext and py_ext > 0
197  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
198  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
199  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
200  else {
201  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
202  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
203  }
204  }
213  {
215  }
216 
217  void getInternalView(vpImage<vpRGBa> &I);
218  void getInternalView(vpImage<unsigned char> &I);
219 
226  vpHomogeneousMatrix get_fMo() const { return fMo; }
227 
228  /* Display functions */
229  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
230  void initScene(const char *obj, const char *desiredObject);
231  void initScene(const vpSceneObject &obj);
232  void initScene(const char *obj);
233 
239  void setCameraColor(const vpColor &col) { camColor = col; }
240 
250  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
251  {
252  constantSamplingTimeMode = _constantSamplingTimeMode;
253  }
254 
261  void setCurrentViewColor(const vpColor &col) { curColor = col; }
262 
269  void setDesiredViewColor(const vpColor &col) { desColor = col; }
270 
277  {
279  }
280 
288  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
296  {
298  }
302  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
303 
314  inline void setSamplingTime(const double &delta_t)
315  {
316  if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
317  this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
318  } else {
319  this->delta_t_ = delta_t;
320  }
321  }
323  void setSingularityManagement(bool sm) { singularityManagement = sm; }
324 
328  void setVerbose(bool verbose) { this->verbose_ = verbose; }
329 
335  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
337 
338 protected:
345 #if defined(_WIN32)
346  static DWORD WINAPI launcher(LPVOID lpParam)
347  {
348  (static_cast<vpRobotWireFrameSimulator *>(lpParam))->updateArticularPosition();
349  return 0;
350  }
351 #elif defined(VISP_HAVE_PTHREAD)
352  static void *launcher(void *arg)
353  {
354  (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
355  // pthread_exit((void*) 0);
356  return NULL;
357  }
358 #endif
359 
360  /* Robot functions */
361  void init() { ; }
364  virtual void updateArticularPosition() = 0;
366  virtual int isInJointLimit() = 0;
369  virtual void computeArticularVelocity() = 0;
370 
371  /* Display functions */
372  void initDisplay() { ; }
373  virtual void initArms() = 0;
374 
376  {
377  m_mutex_artCoord.lock();
378  vpColVector artCoordTmp(6);
379  artCoordTmp = artCoord;
380  m_mutex_artCoord.unlock();
381  return artCoordTmp;
382  }
383  void set_artCoord(const vpColVector &coord)
384  {
385  m_mutex_artCoord.lock();
386  artCoord = coord;
387  m_mutex_artCoord.unlock();
388  }
389 
391  {
392  m_mutex_artVel.lock();
393  vpColVector artVelTmp(artVel);
394  m_mutex_artVel.unlock();
395  return artVelTmp;
396  }
397  void set_artVel(const vpColVector &vel)
398  {
399  m_mutex_artVel.lock();
400  artVel = vel;
401  m_mutex_artVel.unlock();
402  }
403 
405  {
406  m_mutex_velocity.lock();
407  vpColVector velocityTmp = velocity;
408  m_mutex_velocity.unlock();
409  return velocityTmp;
410  }
411  void set_velocity(const vpColVector &vel)
412  {
413  m_mutex_velocity.lock();
414  velocity = vel;
415  m_mutex_velocity.unlock();
416  }
417 
418  void set_displayBusy(const bool &status)
419  {
420  m_mutex_display.lock();
421  displayBusy = status;
422  m_mutex_display.unlock();
423  }
425  {
426  m_mutex_display.lock();
427  bool status = displayBusy;
428  if (!displayBusy)
429  displayBusy = true;
430  m_mutex_display.unlock();
431  return status;
432  }
433 
436  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
438 };
439 
440 #endif
441 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
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:135
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:170
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:178
void unlock()
Definition: vpMutex.h:111
void lock()
Definition: vpMutex.h:95
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()