ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpRobotWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotWireFrameSimulator.cpp 3530 2012-01-03 10:52:12Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 #include <visp/vpConfig.h>
43 
44 
45 
46 #if defined(WIN32) || defined(VISP_HAVE_PTHREAD)
47 #include <visp/vpRobotWireFrameSimulator.h>
48 #include <visp/vpSimulatorViper850.h>
49 
54 {
55  setSamplingTime(0.010);
56  velocity.resize(6);
57  I.resize(480,640);
58  I = 255;
59 #if defined(VISP_HAVE_DISPLAY)
60  display.init(I, 0, 0,"The External view");
61 #endif
62  robotStop = false;
63  jointLimit = false;
64  displayBusy = false;
66  displayAllowed = true;
67  singularityManagement = true;
68  robotArms = NULL;
69 
71  setVelocityCalled = false;
72 
73  verbose_ = false;
74  //pid_t pid = getpid();
75  // setpriority (PRIO_PROCESS, pid, 19);
76 }
77 
83 {
84  setSamplingTime(0.010);
85  velocity.resize(6);
86  I.resize(480,640);
87  I = 255;
88 
90 #if defined(VISP_HAVE_DISPLAY)
91  if (display)
92  this->display.init(I, 0, 0,"The External view");
93 #endif
94  robotStop = false;
95  jointLimit = false;
96  displayBusy = false;
98  singularityManagement = true;
99  robotArms = NULL;
100 
101  constantSamplingTimeMode = false;
102  setVelocityCalled = false;
103 
104  //pid_t pid = getpid();
105  // setpriority (PRIO_PROCESS, pid, 19);
106 }
107 
108 
109 
114 {
115 }
116 
126 void
128 {
129  if(displayCamera){
130  free_Bound_scene (&(this->camera));
131  }
132  vpWireFrameSimulator::initScene(obj, desiredObject);
133  if(displayCamera){
134  free_Bound_scene (&(this->camera));
135  }
136  displayCamera = false;
137 }
138 
149 void
150 vpRobotWireFrameSimulator::initScene(const char* obj, const char* desiredObject)
151 {
152  if(displayCamera){
153  free_Bound_scene (&(this->camera));
154  }
155  vpWireFrameSimulator::initScene(obj, desiredObject);
156  if(displayCamera){
157  free_Bound_scene (&(this->camera));
158  }
159  displayCamera = false;
160 }
161 
170 void
172 {
173  if(displayCamera){
174  free_Bound_scene (&(this->camera));
175  }
177  if(displayCamera){
178  free_Bound_scene (&(this->camera));
179  }
180  displayCamera = false;
181 }
182 
191 void
193 {
194  if(displayCamera){
195  free_Bound_scene (&(this->camera));
196  }
198  if(displayCamera){
199  free_Bound_scene (&(this->camera));
200  }
201  displayCamera = false;
202 }
203 
213 void
215 {
216 
217  if (!sceneInitialized)
218  throw;
219 
220  double u;
221  double v;
222  //if(px_int != 1 && py_int != 1)
223  // we assume px_int and py_int > 0
224  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
225  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
226  {
227  u = (double)I.getWidth()/(2*px_int);
228  v = (double)I.getHeight()/(2*py_int);
229  }
230  else
231  {
232  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
233  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
234  }
235 
236  float o44c[4][4],o44cd[4][4],x,y,z;
237  Matrix id = IDENTITY_MATRIX;
238 
240  get_fMi(fMit);
241  this->cMo = fMit[size_fMi-1].inverse()*fMo;
242  this->cMo = rotz*cMo;
243 
244  vp2jlc_matrix(cMo.inverse(),o44c);
245  vp2jlc_matrix(cdMo.inverse(),o44cd);
246 
247  while (get_displayBusy()) vpTime::wait(2);
248 
249  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
250  x = o44c[2][0] + o44c[3][0];
251  y = o44c[2][1] + o44c[3][1];
252  z = o44c[2][2] + o44c[3][2];
253  add_vwstack ("start","vrp", x,y,z);
254  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
255  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
256  add_vwstack ("start","window", -u, u, -v, v);
257  if (displayObject)
258  display_scene(id,this->scene,I, curColor);
259 
260  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
261  x = o44cd[2][0] + o44cd[3][0];
262  y = o44cd[2][1] + o44cd[3][1];
263  z = o44cd[2][2] + o44cd[3][2];
264  add_vwstack ("start","vrp", x,y,z);
265  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
266  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
267  add_vwstack ("start","window", -u, u, -v, v);
269  {
271  else display_scene(id,desiredScene,I, desColor);
272  }
273  delete[] fMit;
274  set_displayBusy(false);
275 }
276 
286 void
288 {
289 
290  if (!sceneInitialized)
291  throw;
292 
293  double u;
294  double v;
295  //if(px_int != 1 && py_int != 1)
296  // we assume px_int and py_int > 0
297  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
298  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
299  {
300  u = (double)I.getWidth()/(2*px_int);
301  v = (double)I.getHeight()/(2*py_int);
302  }
303  else
304  {
305  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
306  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
307  }
308 
309  float o44c[4][4],o44cd[4][4],x,y,z;
310  Matrix id = IDENTITY_MATRIX;
311 
313  get_fMi(fMit);
314  this->cMo = fMit[size_fMi-1].inverse()*fMo;
315  this->cMo = rotz*cMo;
316 
317  vp2jlc_matrix(cMo.inverse(),o44c);
318  vp2jlc_matrix(cdMo.inverse(),o44cd);
319 
320  while (get_displayBusy()) vpTime::wait(2);
321 
322  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
323  x = o44c[2][0] + o44c[3][0];
324  y = o44c[2][1] + o44c[3][1];
325  z = o44c[2][2] + o44c[3][2];
326  add_vwstack ("start","vrp", x,y,z);
327  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
328  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
329  add_vwstack ("start","window", -u, u, -v, v);
330  if (displayObject)
331  {
332  display_scene(id,this->scene,I, curColor);
333  }
334 
335  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
336  x = o44cd[2][0] + o44cd[3][0];
337  y = o44cd[2][1] + o44cd[3][1];
338  z = o44cd[2][2] + o44cd[3][2];
339  add_vwstack ("start","vrp", x,y,z);
340  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
341  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
342  add_vwstack ("start","window", -u, u, -v, v);
344  {
346  else display_scene(id,desiredScene,I, desColor);
347  }
348  delete[] fMit;
349  set_displayBusy(false);
350 }
351 
359 {
360  vpHomogeneousMatrix cMoTemp;
362  get_fMi(fMit);
363  cMoTemp = fMit[size_fMi-1].inverse()*fMo;
364  delete[] fMit;
365  return cMoTemp;
366 }
367 
368 #endif