Visual Servoing Platform  version 3.0.1
vpRobotWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
41 #include <visp3/robot/vpRobotWireFrameSimulator.h>
42 #include <visp3/robot/vpSimulatorViper850.h>
43 
44 #include "../wireframe-simulator/vpBound.h"
45 #include "../wireframe-simulator/vpVwstack.h"
46 #include "../wireframe-simulator/vpScene.h"
47 
53  I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
54 #if defined(_WIN32)
55 #elif defined(VISP_HAVE_PTHREAD)
56  thread(), attr(),
57 #endif
58  mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
59  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
60  cameraParam(),
61 #if defined(VISP_HAVE_DISPLAY)
62  display(),
63 #endif
64  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
65  setVelocityCalled(false), verbose_(false)
66 {
67  setSamplingTime(0.010);
68  velocity.resize(6);
69  I.resize(480,640);
70  I = 255;
71 #if defined(VISP_HAVE_DISPLAY)
72  display.init(I, 0, 0,"The External view");
73 #endif
74 
75  //pid_t pid = getpid();
76  // setpriority (PRIO_PROCESS, pid, 19);
77 }
78 
85  I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
86 #if defined(_WIN32)
87 #elif defined(VISP_HAVE_PTHREAD)
88  thread(), attr(),
89 #endif
90  /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
91  displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
92  cameraParam(),
93 #if defined(VISP_HAVE_DISPLAY)
94  display(),
95 #endif
97  setVelocityCalled(false), verbose_(false)
98 {
99  setSamplingTime(0.010);
100  velocity.resize(6);
101  I.resize(480,640);
102  I = 255;
103 
104 #if defined(VISP_HAVE_DISPLAY)
105  if (do_display)
106  this->display.init(I, 0, 0,"The External view");
107 #endif
108 
109  //pid_t pid = getpid();
110  // setpriority (PRIO_PROCESS, pid, 19);
111 }
112 
113 
114 
119 {
120 }
121 
131 void
133 {
134  if(displayCamera){
135  free_Bound_scene (&(this->camera));
136  }
137  vpWireFrameSimulator::initScene(obj, desired_object);
138  if(displayCamera){
139  free_Bound_scene (&(this->camera));
140  }
141  displayCamera = false;
142 }
143 
154 void
155 vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
156 {
157  if(displayCamera){
158  free_Bound_scene (&(this->camera));
159  }
160  vpWireFrameSimulator::initScene(obj, desired_object);
161  if(displayCamera){
162  free_Bound_scene (&(this->camera));
163  }
164  displayCamera = false;
165 }
166 
175 void
177 {
178  if(displayCamera){
179  free_Bound_scene (&(this->camera));
180  }
182  if(displayCamera){
183  free_Bound_scene (&(this->camera));
184  }
185  displayCamera = false;
186 }
187 
196 void
198 {
199  if(displayCamera){
200  free_Bound_scene (&(this->camera));
201  }
203  if(displayCamera){
204  free_Bound_scene (&(this->camera));
205  }
206  displayCamera = false;
207 }
208 
218 void
220 {
221 
222  if (!sceneInitialized)
223  throw;
224 
225  double u;
226  double v;
227  //if(px_int != 1 && py_int != 1)
228  // we assume px_int and py_int > 0
229  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
230  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
231  {
232  u = (double)I_.getWidth()/(2*px_int);
233  v = (double)I_.getHeight()/(2*py_int);
234  }
235  else
236  {
237  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
238  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
239  }
240 
241  float o44c[4][4],o44cd[4][4],x,y,z;
242  Matrix id = IDENTITY_MATRIX;
243 
245  get_fMi(fMit);
246  this->cMo = fMit[size_fMi-1].inverse()*fMo;
247  this->cMo = rotz*cMo;
248 
249  vp2jlc_matrix(cMo.inverse(),o44c);
250  vp2jlc_matrix(cdMo.inverse(),o44cd);
251 
252  while (get_displayBusy()) vpTime::wait(2);
253 
254  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
255  x = o44c[2][0] + o44c[3][0];
256  y = o44c[2][1] + o44c[3][1];
257  z = o44c[2][2] + o44c[3][2];
258  add_vwstack ("start","vrp", x,y,z);
259  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
260  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
261  add_vwstack ("start","window", -u, u, -v, v);
262  if (displayObject)
263  display_scene(id,this->scene,I_, curColor);
264 
265  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
266  x = o44cd[2][0] + o44cd[3][0];
267  y = o44cd[2][1] + o44cd[3][1];
268  z = o44cd[2][2] + o44cd[3][2];
269  add_vwstack ("start","vrp", x,y,z);
270  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
271  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
272  add_vwstack ("start","window", -u, u, -v, v);
274  {
276  else display_scene(id,desiredScene,I_, desColor);
277  }
278  delete[] fMit;
279  set_displayBusy(false);
280 }
281 
291 void
293 {
294 
295  if (!sceneInitialized)
296  throw;
297 
298  double u;
299  double v;
300  //if(px_int != 1 && py_int != 1)
301  // we assume px_int and py_int > 0
302  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
303  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
304  {
305  u = (double)I.getWidth()/(2*px_int);
306  v = (double)I.getHeight()/(2*py_int);
307  }
308  else
309  {
310  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
311  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
312  }
313 
314  float o44c[4][4],o44cd[4][4],x,y,z;
315  Matrix id = IDENTITY_MATRIX;
316 
318  get_fMi(fMit);
319  this->cMo = fMit[size_fMi-1].inverse()*fMo;
320  this->cMo = rotz*cMo;
321 
322  vp2jlc_matrix(cMo.inverse(),o44c);
323  vp2jlc_matrix(cdMo.inverse(),o44cd);
324 
325  while (get_displayBusy()) vpTime::wait(2);
326 
327  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
328  x = o44c[2][0] + o44c[3][0];
329  y = o44c[2][1] + o44c[3][1];
330  z = o44c[2][2] + o44c[3][2];
331  add_vwstack ("start","vrp", x,y,z);
332  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
333  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
334  add_vwstack ("start","window", -u, u, -v, v);
335  if (displayObject)
336  {
337  display_scene(id,this->scene,I_, curColor);
338  }
339 
340  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
341  x = o44cd[2][0] + o44cd[3][0];
342  y = o44cd[2][1] + o44cd[3][1];
343  z = o44cd[2][2] + o44cd[3][2];
344  add_vwstack ("start","vrp", x,y,z);
345  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
346  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
347  add_vwstack ("start","window", -u, u, -v, v);
349  {
351  else display_scene(id,desiredScene,I_, desColor);
352  }
353  delete[] fMit;
354  set_displayBusy(false);
355 }
356 
364 {
365  vpHomogeneousMatrix cMoTemp;
367  get_fMi(fMit);
368  cMoTemp = fMit[size_fMi-1].inverse()*fMo;
369  delete[] fMit;
370  return cMoTemp;
371 }
372 
373 #elif !defined(VISP_BUILD_SHARED_LIBS)
374 // Work arround to avoid warning: libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
375 void dummy_vpRobotWireFrameSimulator() {};
376 #endif
virtual void get_fMi(vpHomogeneousMatrix *fMit)=0
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Definition: vpDisplayX.cpp:254
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
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 inverse() const
vpHomogeneousMatrix cdMo
vpHomogeneousMatrix fMo
static const vpColor red
Definition: vpColor.h:163
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
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)
resize the image : Image initialization
Definition: vpImage.h:903
vpHomogeneousMatrix cMo
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
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)
unsigned int getHeight() const
Definition: vpImage.h:175
vpHomogeneousMatrix rotz
unsigned int getWidth() const
Definition: vpImage.h:226
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225