ViSP  3.0.0
vpRobotWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
41 #include <visp3/robot/vpRobotWireFrameSimulator.h>
42 #include <visp3/robot/vpSimulatorViper850.h>
43 
49  I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
50 #if defined(_WIN32)
51 #elif defined(VISP_HAVE_PTHREAD)
52  thread(), attr(),
53 #endif
54  mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
55  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
56  cameraParam(),
57 #if defined(VISP_HAVE_DISPLAY)
58  display(),
59 #endif
60  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
61  setVelocityCalled(false), verbose_(false)
62 {
63  setSamplingTime(0.010);
64  velocity.resize(6);
65  I.resize(480,640);
66  I = 255;
67 #if defined(VISP_HAVE_DISPLAY)
68  display.init(I, 0, 0,"The External view");
69 #endif
70 
71  //pid_t pid = getpid();
72  // setpriority (PRIO_PROCESS, pid, 19);
73 }
74 
81  I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
82 #if defined(_WIN32)
83 #elif defined(VISP_HAVE_PTHREAD)
84  thread(), attr(),
85 #endif
86  /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
87  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
88  cameraParam(),
89 #if defined(VISP_HAVE_DISPLAY)
90  display(),
91 #endif
93  setVelocityCalled(false), verbose_(false)
94 {
95  setSamplingTime(0.010);
96  velocity.resize(6);
97  I.resize(480,640);
98  I = 255;
99 
100 #if defined(VISP_HAVE_DISPLAY)
101  if (do_display)
102  this->display.init(I, 0, 0,"The External view");
103 #endif
104 
105  //pid_t pid = getpid();
106  // setpriority (PRIO_PROCESS, pid, 19);
107 }
108 
109 
110 
115 {
116 }
117 
127 void
129 {
130  if(displayCamera){
131  free_Bound_scene (&(this->camera));
132  }
133  vpWireFrameSimulator::initScene(obj, desired_object);
134  if(displayCamera){
135  free_Bound_scene (&(this->camera));
136  }
137  displayCamera = false;
138 }
139 
150 void
151 vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
152 {
153  if(displayCamera){
154  free_Bound_scene (&(this->camera));
155  }
156  vpWireFrameSimulator::initScene(obj, desired_object);
157  if(displayCamera){
158  free_Bound_scene (&(this->camera));
159  }
160  displayCamera = false;
161 }
162 
171 void
173 {
174  if(displayCamera){
175  free_Bound_scene (&(this->camera));
176  }
178  if(displayCamera){
179  free_Bound_scene (&(this->camera));
180  }
181  displayCamera = false;
182 }
183 
192 void
194 {
195  if(displayCamera){
196  free_Bound_scene (&(this->camera));
197  }
199  if(displayCamera){
200  free_Bound_scene (&(this->camera));
201  }
202  displayCamera = false;
203 }
204 
214 void
216 {
217 
218  if (!sceneInitialized)
219  throw;
220 
221  double u;
222  double v;
223  //if(px_int != 1 && py_int != 1)
224  // we assume px_int and py_int > 0
225  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
226  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
227  {
228  u = (double)I_.getWidth()/(2*px_int);
229  v = (double)I_.getHeight()/(2*py_int);
230  }
231  else
232  {
233  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
234  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
235  }
236 
237  float o44c[4][4],o44cd[4][4],x,y,z;
238  Matrix id = IDENTITY_MATRIX;
239 
241  get_fMi(fMit);
242  this->cMo = fMit[size_fMi-1].inverse()*fMo;
243  this->cMo = rotz*cMo;
244 
245  vp2jlc_matrix(cMo.inverse(),o44c);
246  vp2jlc_matrix(cdMo.inverse(),o44cd);
247 
248  while (get_displayBusy()) vpTime::wait(2);
249 
250  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
251  x = o44c[2][0] + o44c[3][0];
252  y = o44c[2][1] + o44c[3][1];
253  z = o44c[2][2] + o44c[3][2];
254  add_vwstack ("start","vrp", x,y,z);
255  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
256  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
257  add_vwstack ("start","window", -u, u, -v, v);
258  if (displayObject)
259  display_scene(id,this->scene,I_, curColor);
260 
261  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
262  x = o44cd[2][0] + o44cd[3][0];
263  y = o44cd[2][1] + o44cd[3][1];
264  z = o44cd[2][2] + o44cd[3][2];
265  add_vwstack ("start","vrp", x,y,z);
266  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
267  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
268  add_vwstack ("start","window", -u, u, -v, v);
270  {
272  else display_scene(id,desiredScene,I_, desColor);
273  }
274  delete[] fMit;
275  set_displayBusy(false);
276 }
277 
287 void
289 {
290 
291  if (!sceneInitialized)
292  throw;
293 
294  double u;
295  double v;
296  //if(px_int != 1 && py_int != 1)
297  // we assume px_int and py_int > 0
298  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
299  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
300  {
301  u = (double)I.getWidth()/(2*px_int);
302  v = (double)I.getHeight()/(2*py_int);
303  }
304  else
305  {
306  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
307  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
308  }
309 
310  float o44c[4][4],o44cd[4][4],x,y,z;
311  Matrix id = IDENTITY_MATRIX;
312 
314  get_fMi(fMit);
315  this->cMo = fMit[size_fMi-1].inverse()*fMo;
316  this->cMo = rotz*cMo;
317 
318  vp2jlc_matrix(cMo.inverse(),o44c);
319  vp2jlc_matrix(cdMo.inverse(),o44cd);
320 
321  while (get_displayBusy()) vpTime::wait(2);
322 
323  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
324  x = o44c[2][0] + o44c[3][0];
325  y = o44c[2][1] + o44c[3][1];
326  z = o44c[2][2] + o44c[3][2];
327  add_vwstack ("start","vrp", x,y,z);
328  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
329  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
330  add_vwstack ("start","window", -u, u, -v, v);
331  if (displayObject)
332  {
333  display_scene(id,this->scene,I_, curColor);
334  }
335 
336  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
337  x = o44cd[2][0] + o44cd[3][0];
338  y = o44cd[2][1] + o44cd[3][1];
339  z = o44cd[2][2] + o44cd[3][2];
340  add_vwstack ("start","vrp", x,y,z);
341  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
342  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
343  add_vwstack ("start","window", -u, u, -v, v);
345  {
347  else display_scene(id,desiredScene,I_, desColor);
348  }
349  delete[] fMit;
350  set_displayBusy(false);
351 }
352 
360 {
361  vpHomogeneousMatrix cMoTemp;
363  get_fMi(fMit);
364  cMoTemp = fMit[size_fMi-1].inverse()*fMo;
365  delete[] fMit;
366  return cMoTemp;
367 }
368 
369 #elif !defined(VISP_BUILD_SHARED_LIBS)
370 // Work arround to avoid warning: libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
371 void dummy_vpRobotWireFrameSimulator() {};
372 #endif
virtual void get_fMi(vpHomogeneousMatrix *fMit)=0
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
unsigned int getWidth() const
Definition: vpImage.h:161
Implementation of an homogeneous matrix and operations on such kind of matrices.
A cylindrical tool is attached to the camera.
bool constantSamplingTimeMode
Flag used to force the sampling time in the thread computing the robot&#39;s displacement to a constant v...
vpHomogeneousMatrix cdMo
vpHomogeneousMatrix fMo
static const vpColor red
Definition: vpColor.h:163
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:141
This class aims to be a basis used to create all the robot simulators.
void set_displayBusy(const bool &status)
void setSamplingTime(const double &delta_t)
void getInternalView(vpImage< vpRGBa > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void resize(const unsigned int h, const unsigned int w)
set the size of the image without initializing it.
Definition: vpImage.h:616
vpHomogeneousMatrix cMo
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:152
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpSceneDesiredObject desiredObject
bool setVelocityCalled
Flag used to specify to the thread managing the robot displacements that the setVelocity() method has...
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix rotz
unsigned int getHeight() const
Definition: vpImage.h:152
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Definition: vpDisplayX.cpp:190
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217