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(10);
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  //pid_t pid = getpid();
74  // setpriority (PRIO_PROCESS, pid, 19);
75 }
76 
77 
79 {
80  setSamplingTime(0.010);
81  velocity.resize(6);
82  I.resize(480,640);
83  I = 255;
84 
86 #if defined(VISP_HAVE_DISPLAY)
87  if (display)
88  this->display.init(I, 0, 0,"The External view");
89 #endif
90  robotStop = false;
91  jointLimit = false;
92  displayBusy = false;
94  singularityManagement = true;
95  robotArms = NULL;
96 
98  setVelocityCalled = false;
99 
100  //pid_t pid = getpid();
101  // setpriority (PRIO_PROCESS, pid, 19);
102 }
103 
104 
105 
110 {
111 }
112 
122 void
124 {
125  if(displayCamera){
126  free_Bound_scene (&(this->camera));
127  }
128  vpWireFrameSimulator::initScene(obj, desiredObject);
129  if(displayCamera){
130  free_Bound_scene (&(this->camera));
131  }
132  displayCamera = false;
133 }
134 
145 void
146 vpRobotWireFrameSimulator::initScene(const char* obj, const char* desiredObject)
147 {
148  if(displayCamera){
149  free_Bound_scene (&(this->camera));
150  }
151  vpWireFrameSimulator::initScene(obj, desiredObject);
152  if(displayCamera){
153  free_Bound_scene (&(this->camera));
154  }
155  displayCamera = false;
156 }
157 
166 void
168 {
169  if(displayCamera){
170  free_Bound_scene (&(this->camera));
171  }
173  if(displayCamera){
174  free_Bound_scene (&(this->camera));
175  }
176  displayCamera = false;
177 }
178 
187 void
189 {
190  if(displayCamera){
191  free_Bound_scene (&(this->camera));
192  }
194  if(displayCamera){
195  free_Bound_scene (&(this->camera));
196  }
197  displayCamera = false;
198 }
199 
209 void
211 {
212 
213  if (!sceneInitialized)
214  throw;
215 
216  double u;
217  double v;
218  //if(px_int != 1 && py_int != 1)
219  // we assume px_int and py_int > 0
220  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
221  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
222  {
223  u = (double)I.getWidth()/(2*px_int);
224  v = (double)I.getHeight()/(2*py_int);
225  }
226  else
227  {
228  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
229  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
230  }
231 
232  float o44c[4][4],o44cd[4][4],x,y,z;
233  Matrix id = IDENTITY_MATRIX;
234 
236  get_fMi(fMit);
237  this->cMo = fMit[size_fMi-1].inverse()*fMo;
238  this->cMo = rotz*cMo;
239 
240  vp2jlc_matrix(cMo.inverse(),o44c);
241  vp2jlc_matrix(cdMo.inverse(),o44cd);
242 
243  while (get_displayBusy()) vpTime::wait(2);
244 
245  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
246  x = o44c[2][0] + o44c[3][0];
247  y = o44c[2][1] + o44c[3][1];
248  z = o44c[2][2] + o44c[3][2];
249  add_vwstack ("start","vrp", x,y,z);
250  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
251  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
252  add_vwstack ("start","window", -u, u, -v, v);
253  if (displayObject)
254  display_scene(id,this->scene,I, curColor);
255 
256  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
257  x = o44cd[2][0] + o44cd[3][0];
258  y = o44cd[2][1] + o44cd[3][1];
259  z = o44cd[2][2] + o44cd[3][2];
260  add_vwstack ("start","vrp", x,y,z);
261  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
262  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
263  add_vwstack ("start","window", -u, u, -v, v);
265  {
267  else display_scene(id,desiredScene,I, desColor);
268  }
269  delete[] fMit;
270  set_displayBusy(false);
271 }
272 
282 void
284 {
285 
286  if (!sceneInitialized)
287  throw;
288 
289  double u;
290  double v;
291  //if(px_int != 1 && py_int != 1)
292  // we assume px_int and py_int > 0
293  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
294  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
295  {
296  u = (double)I.getWidth()/(2*px_int);
297  v = (double)I.getHeight()/(2*py_int);
298  }
299  else
300  {
301  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
302  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
303  }
304 
305  float o44c[4][4],o44cd[4][4],x,y,z;
306  Matrix id = IDENTITY_MATRIX;
307 
309  get_fMi(fMit);
310  this->cMo = fMit[size_fMi-1].inverse()*fMo;
311  this->cMo = rotz*cMo;
312 
313  vp2jlc_matrix(cMo.inverse(),o44c);
314  vp2jlc_matrix(cdMo.inverse(),o44cd);
315 
316  while (get_displayBusy()) vpTime::wait(2);
317 
318  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
319  x = o44c[2][0] + o44c[3][0];
320  y = o44c[2][1] + o44c[3][1];
321  z = o44c[2][2] + o44c[3][2];
322  add_vwstack ("start","vrp", x,y,z);
323  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
324  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
325  add_vwstack ("start","window", -u, u, -v, v);
326  if (displayObject)
327  {
328  display_scene(id,this->scene,I, curColor);
329  }
330 
331  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
332  x = o44cd[2][0] + o44cd[3][0];
333  y = o44cd[2][1] + o44cd[3][1];
334  z = o44cd[2][2] + o44cd[3][2];
335  add_vwstack ("start","vrp", x,y,z);
336  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
337  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
338  add_vwstack ("start","window", -u, u, -v, v);
340  {
342  else display_scene(id,desiredScene,I, desColor);
343  }
344  delete[] fMit;
345  set_displayBusy(false);
346 }
347 
355 {
356  vpHomogeneousMatrix cMoTemp;
358  get_fMi(fMit);
359  cMoTemp = fMit[size_fMi-1].inverse()*fMo;
360  delete[] fMit;
361  return cMoTemp;
362 }
363 
364 #endif