Visual Servoing Platform  version 3.6.1 under development (2024-12-03)
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 
39 #ifndef vpRobotWireFrameSimulator_HH
40 #define vpRobotWireFrameSimulator_HH
41 
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 
61 BEGIN_VISP_NAMESPACE
79 class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
80 {
81 public:
83 
84  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
85 
86 protected:
88  double tcur;
90  double tprev;
91 
93  Bound_scene *robotArms;
94 
96  unsigned int size_fMi;
103  vpHomogeneousMatrix *fMi;
104 
106  vpColVector artCoord;
108  vpColVector artVel;
110  vpColVector velocity;
111 
112  std::thread *m_thread;
113 
114  std::mutex m_mutex_fMi;
115  std::mutex m_mutex_eMc;
116  std::mutex m_mutex_artVel;
117  std::mutex m_mutex_artCoord;
118  std::mutex m_mutex_velocity;
119  std::mutex m_mutex_display;
120  std::mutex m_mutex_robotStop;
121  std::mutex m_mutex_frame;
122  std::mutex m_mutex_setVelocityCalled;
123  std::mutex m_mutex_scene;
124 
125  bool displayBusy;
126 
128  bool robotStop;
130  bool jointLimit;
132  unsigned int jointLimitArt;
134  bool singularityManagement;
135 
137  vpCameraParameters cameraParam;
138 
139 #if defined(VISP_HAVE_X11)
140  vpDisplayX display;
141 #elif defined(VISP_HAVE_GDI)
142  vpDisplayGDI display;
143 #elif defined(HAVE_OPENCV_HIGHGUI)
144  vpDisplayOpenCV display;
145 #elif defined(VISP_HAVE_D3D9)
146  vpDisplayD3D display;
147 #elif defined(VISP_HAVE_GTK)
148  vpDisplayGTK display;
149 #endif
150 
151  vpDisplayRobotType displayType;
152 
153  bool displayAllowed;
158  bool constantSamplingTimeMode;
159 
162  bool setVelocityCalled;
163 
164  bool verbose_;
165 
166 public:
167  vpRobotWireFrameSimulator();
168  VP_EXPLICIT vpRobotWireFrameSimulator(bool display);
169 
178  {
179  // if(px_ext != 1 && py_ext != 1)
180  // we assume px_ext and py_ext > 0
181  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
182  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
183  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
184  else {
185  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
186  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
187  }
188  }
197  {
199  }
200 
201  void getInternalView(vpImage<vpRGBa> &I);
202  void getInternalView(vpImage<unsigned char> &I);
203 
210  vpHomogeneousMatrix get_fMo() const { return fMo; }
211 
212  /* Display functions */
213  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
214  void initScene(const char *obj, const char *desiredObject);
215  void initScene(const vpSceneObject &obj);
216  void initScene(const char *obj);
217 
223  void setCameraColor(const vpColor &col) { camColor = col; }
224 
234  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
235  {
236  constantSamplingTimeMode = _constantSamplingTimeMode;
237  }
238 
245  void setCurrentViewColor(const vpColor &col) { curColor = col; }
246 
253  void setDesiredViewColor(const vpColor &col) { desColor = col; }
254 
261  {
263  }
264 
272  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
280  {
282  }
286  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
287 
297  inline void setSamplingTime(const double &delta_t)
298  {
299  if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
300  this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
301  }
302  else {
303  this->delta_t_ = delta_t;
304  }
305  }
309  void setSingularityManagement(bool sm) { singularityManagement = sm; }
310 
314  void setVerbose(bool verbose) { this->verbose_ = verbose; }
315 
321  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
323 
324 protected:
331  static void launcher(vpRobotWireFrameSimulator &simulator)
332  {
333  simulator.updateArticularPosition();
334  }
335 
340  virtual void updateArticularPosition() = 0;
342  virtual int isInJointLimit() = 0;
345  virtual void computeArticularVelocity() = 0;
346 
347  /* Display functions */
348  void initDisplay() { ; }
349  virtual void initArms() = 0;
350 
351  vpColVector get_artCoord()
352  {
353  m_mutex_artCoord.lock();
354  vpColVector artCoordTmp(6);
355  artCoordTmp = artCoord;
356  m_mutex_artCoord.unlock();
357  return artCoordTmp;
358  }
359  void set_artCoord(const vpColVector &coord)
360  {
361  m_mutex_artCoord.lock();
362  artCoord = coord;
363  m_mutex_artCoord.unlock();
364  }
365 
366  vpColVector get_artVel()
367  {
368  m_mutex_artVel.lock();
369  vpColVector artVelTmp(artVel);
370  m_mutex_artVel.unlock();
371  return artVelTmp;
372  }
373  void set_artVel(const vpColVector &vel)
374  {
375  m_mutex_artVel.lock();
376  artVel = vel;
377  m_mutex_artVel.unlock();
378  }
379 
380  vpColVector get_velocity()
381  {
382  m_mutex_velocity.lock();
383  vpColVector velocityTmp = velocity;
384  m_mutex_velocity.unlock();
385  return velocityTmp;
386  }
387  void set_velocity(const vpColVector &vel)
388  {
389  m_mutex_velocity.lock();
390  velocity = vel;
391  m_mutex_velocity.unlock();
392  }
393 
394  void set_displayBusy(const bool &status)
395  {
396  m_mutex_display.lock();
397  displayBusy = status;
398  m_mutex_display.unlock();
399  }
400  bool get_displayBusy()
401  {
402  m_mutex_display.lock();
403  bool status = displayBusy;
404  if (!displayBusy)
405  displayBusy = true;
406  m_mutex_display.unlock();
407  return status;
408  }
409 
412  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
414 };
415 END_VISP_NAMESPACE
416 #endif
417 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:106
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:130
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:133
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:262
This class aims to be a basis used to create all the robot simulators.
virtual void setSamplingTime(const double &delta_t)
void setVerbose(bool verbose)
Definition: vpRobot.h:170
bool verbose_
Definition: vpRobot.h:118
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)
void setCurrentViewColor(const vpColor &col)
vpHomogeneousMatrix get_cMo() const
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
vpHomogeneousMatrix fMo
void setCameraColor(const vpColor &col)
void setDesiredViewColor(const vpColor &col)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setGraphicsThickness(unsigned int thickness)
VISP_EXPORT double getMinTimeForUsleepCall()