ViSP  2.9.0
vpRobotWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * $Id: vpRobotWireFrameSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Basic class used to make robot simulators.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 #ifndef vpRobotWireFrameSimulator_HH
43 #define vpRobotWireFrameSimulator_HH
44 
50 #include <visp/vpConfig.h>
51 
52 
53 
54 #if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
55 #include <cmath> // std::fabs
56 #include <limits> // numeric_limits
57 #if defined(_WIN32)
58 # include <windows.h>
59 #elif defined(VISP_HAVE_PTHREAD)
60 # include <pthread.h>
61 #endif
62 
63 #include <visp/vpWireFrameSimulator.h>
64 #include <visp/vpRobot.h>
65 #include <visp/vpRobotSimulator.h>
66 #include <visp/vpDisplayOpenCV.h>
67 #include <visp/vpDisplayX.h>
68 #include <visp/vpDisplayGTK.h>
69 #include <visp/vpDisplayGDI.h>
70 #include <visp/vpDisplayD3D.h>
71 
90 {
91  public:
93 
94  typedef enum
95  {
97  MODEL_DH
98  } vpDisplayRobotType;
99 
100 
101  protected:
103  double tcur;
105  double tprev;
106 
108  Bound_scene* robotArms;
109 
111  unsigned int size_fMi;
116 
123 
124 #if defined(_WIN32)
125  HANDLE hThread;
126  HANDLE mutex_fMi;
127  HANDLE mutex_artVel;
128  HANDLE mutex_artCoord;
129  HANDLE mutex_velocity;
130  HANDLE mutex_display;
131 #elif defined(VISP_HAVE_PTHREAD)
132  pthread_t thread;
133  pthread_attr_t attr;
134  pthread_mutex_t mutex_fMi;
135  pthread_mutex_t mutex_artVel;
136  pthread_mutex_t mutex_artCoord;
137  pthread_mutex_t mutex_velocity;
138  pthread_mutex_t mutex_display;
139 #endif
140 
142 
144  bool robotStop;
148  unsigned int jointLimitArt;
151 
154 
155  #if defined VISP_HAVE_X11
157  #elif defined VISP_HAVE_GDI
158  vpDisplayGDI display;
159  #elif defined VISP_HAVE_OPENCV
160  vpDisplayOpenCV display;
161  #elif defined VISP_HAVE_D3D9
162  vpDisplayD3D display;
163  #elif defined VISP_HAVE_GTK
164  vpDisplayGTK display;
165  #endif
166 
168 
172 
175 
176  bool verbose_;
177 
178  public:
180  vpRobotWireFrameSimulator(bool display);
181  virtual ~vpRobotWireFrameSimulator();
182 
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  {
196  unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
197  return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
198  }
199  }
206  {
208  }
209 
210  void getInternalView(vpImage<vpRGBa> &I);
211  void getInternalView(vpImage<unsigned char> &I);
212 
219  vpHomogeneousMatrix get_fMo() const {return fMo;}
220 
221  /* Display functions */
222  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
223  void initScene(const char* obj, const char* desiredObject);
224  void initScene (const vpSceneObject &obj);
225  void initScene(const char* obj);
226 
232  void setCameraColor(const vpColor col) {camColor = col;}
233 
242  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
243  constantSamplingTimeMode = _constantSamplingTimeMode;
244  }
245 
251  void setCurrentViewColor(const vpColor col) {curColor = col;}
252 
258  void setDesiredViewColor(const vpColor col) {desColor = col;}
259 
266  {
268  }
269 
275  inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
282  {
284  }
288  void setGraphicsThickness(unsigned int thickness)
289  {
290  this->thickness_ = thickness;
291  }
292 
302  inline void setSamplingTime(const double &delta_t)
303  {
304  if(delta_t < static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3)){
305  this->delta_t_ = static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3);
306  } else {
307  this->delta_t_ = delta_t;
308  }
309  }
311  void setSingularityManagement (const bool sm) {singularityManagement = sm;}
312 
316  void setVerbose(bool verbose)
317  {
318  this->verbose_ = verbose;
319  }
320 
326  void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;}
327 
328  protected:
332  #if defined(_WIN32)
333  static DWORD WINAPI launcher( LPVOID lpParam )
334  {
335  ((vpRobotWireFrameSimulator *)lpParam)->updateArticularPosition();
336  return 0;
337  }
338  #elif defined(VISP_HAVE_PTHREAD)
339  static void* launcher(void *arg)
340  {
341  (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
342  // pthread_exit((void*) 0);
343  return NULL;
344  }
345  #endif
346 
347  /* Robot functions */
348  void init() {;}
350  virtual void updateArticularPosition() = 0;
352  virtual int isInJointLimit () = 0;
354  virtual void computeArticularVelocity() = 0;
355 
356  /* Display functions */
357  void initDisplay() {;}
358  virtual void initArms() = 0;
359 
360  #if defined(_WIN32)
361  vpColVector get_artCoord() const {
362  WaitForSingleObject(mutex_artCoord,INFINITE);
363  vpColVector artCoordTmp (6);
364  artCoordTmp = artCoord;
365  ReleaseMutex(mutex_artCoord);
366  return artCoordTmp;}
367  void set_artCoord(const vpColVector &coord) {
368  WaitForSingleObject(mutex_artCoord,INFINITE);
369  artCoord = coord;
370  ReleaseMutex(mutex_artCoord);}
371 
372  vpColVector get_artVel() const {
373  WaitForSingleObject(mutex_artVel,INFINITE);
374  vpColVector artVelTmp (artVel);
375  ReleaseMutex(mutex_artVel);
376  return artVelTmp;}
377  void set_artVel(const vpColVector &vel) {
378  WaitForSingleObject(mutex_artVel,INFINITE);
379  artVel = vel;
380  ReleaseMutex(mutex_artVel);}
381 
382  vpColVector get_velocity() {
383  WaitForSingleObject(mutex_velocity,INFINITE);
384  vpColVector velocityTmp = velocity;
385  ReleaseMutex(mutex_velocity);
386  return velocityTmp;}
387  void set_velocity(const vpColVector &vel) {
388  WaitForSingleObject(mutex_velocity,INFINITE);
389  velocity = vel;
390  ReleaseMutex(mutex_velocity);}
391 
392  void set_displayBusy (const bool &status) {
393  WaitForSingleObject(mutex_display,INFINITE);
394  displayBusy = status;
395  ReleaseMutex(mutex_display);}
396  bool get_displayBusy () {
397  WaitForSingleObject(mutex_display,INFINITE);
398  bool status = displayBusy;
399  if (!displayBusy) displayBusy = true;
400  ReleaseMutex(mutex_display);
401  return status;}
402 
403  #elif defined(VISP_HAVE_PTHREAD)
405  pthread_mutex_lock (&mutex_artCoord);
406  vpColVector artCoordTmp (6);
407  artCoordTmp = artCoord;
408  pthread_mutex_unlock (&mutex_artCoord);
409  return artCoordTmp;}
410  void set_artCoord(const vpColVector &coord) {
411  pthread_mutex_lock (&mutex_artCoord);
412  artCoord = coord;
413  pthread_mutex_unlock (&mutex_artCoord);}
414 
416  pthread_mutex_lock (&mutex_artVel);
417  vpColVector artVelTmp (artVel);
418  pthread_mutex_unlock (&mutex_artVel);
419  return artVelTmp;}
420  void set_artVel(const vpColVector &vel) {
421  pthread_mutex_lock (&mutex_artVel);
422  artVel = vel;
423  pthread_mutex_unlock (&mutex_artVel);}
424 
426  pthread_mutex_lock (&mutex_velocity);
427  vpColVector velocityTmp = velocity;
428  pthread_mutex_unlock (&mutex_velocity);
429  return velocityTmp;}
430  void set_velocity(const vpColVector &vel) {
431  pthread_mutex_lock (&mutex_velocity);
432  velocity = vel;
433  pthread_mutex_unlock (&mutex_velocity);}
434 
435  void set_displayBusy (const bool &status) {
436  pthread_mutex_lock (&mutex_display);
437  displayBusy = status;
438  pthread_mutex_unlock (&mutex_display);}
439  bool get_displayBusy () {
440  pthread_mutex_lock (&mutex_display);
441  bool status = displayBusy;
442  if (!displayBusy) displayBusy = true;
443  pthread_mutex_unlock (&mutex_display);
444  return status;}
445  #endif
446 
448  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
449 };
450 
451 #endif
452 
453 #endif
void setDisplayRobotType(const vpDisplayRobotType dispType)
void setSingularityManagement(const bool sm)
unsigned int getWidth() const
Definition: vpImage.h:159
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static void * launcher(void *arg)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void set_fMo(const vpHomogeneousMatrix &fMo_)
bool constantSamplingTimeMode
Flag used to force the sampling time in the thread computing the robot's displacement to a constant v...
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void setGraphicsThickness(unsigned int thickness)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
vpHomogeneousMatrix fMo
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
vpHomogeneousMatrix get_fMo() const
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
This class aims to be a basis used to create all the robot simulators.
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
void set_displayBusy(const bool &status)
The vpDisplayOpenCV allows to display image using the opencv library.
void setSamplingTime(const double &delta_t)
Generic class defining intrinsic camera parameters.
void setCameraColor(const vpColor col)
vpHomogeneousMatrix getExternalCameraPosition() const
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:148
vpCameraParameters getExternalCameraParameters() const
void set_velocity(const vpColVector &vel)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void setCurrentViewColor(const vpColor col)
This class aims to be a basis used to create all the simulators of robots.
bool setVelocityCalled
Flag used to specify to the thread managing the robot displacements that the setVelocity() method has...
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
vpHomogeneousMatrix get_cMo() const
void setDesiredViewColor(const vpColor col)
void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo_)
unsigned int getHeight() const
Definition: vpImage.h:150
void set_artCoord(const vpColVector &coord)
void set_artVel(const vpColVector &vel)
static double minTimeForUsleepCall
Definition: vpTime.h:82
vpHomogeneousMatrix getExternalCameraPosition() const