Visual Servoing Platform  version 3.4.0
vpRobotWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Basic class used to make robot simulators.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
42 #include <visp3/robot/vpRobotWireFrameSimulator.h>
43 #include <visp3/robot/vpSimulatorViper850.h>
44 
45 #include "../wireframe-simulator/vpBound.h"
46 #include "../wireframe-simulator/vpScene.h"
47 #include "../wireframe-simulator/vpVwstack.h"
48 
53  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
54  artCoord(), artVel(), velocity(),
55 #if defined(_WIN32)
56 #elif defined(VISP_HAVE_PTHREAD)
57  thread(), attr(),
58 #endif
59  mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(), displayBusy(false),
60  robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true), cameraParam(),
61 #if defined(VISP_HAVE_DISPLAY)
62  display(),
63 #endif
64  displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false), setVelocityCalled(false),
65  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 
84  : vpWireFrameSimulator(), vpRobotSimulator(), I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
85  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  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 
117 
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 
155 void 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 
180 {
181  if (displayCamera) {
182  free_Bound_scene(&(this->camera));
183  }
185  if (displayCamera) {
186  free_Bound_scene(&(this->camera));
187  }
188  displayCamera = false;
189 }
190 
202 {
203  if (displayCamera) {
204  free_Bound_scene(&(this->camera));
205  }
207  if (displayCamera) {
208  free_Bound_scene(&(this->camera));
209  }
210  displayCamera = false;
211 }
212 
225 {
226 
227  if (!sceneInitialized)
228  throw;
229 
230  double u;
231  double v;
232  // if(px_int != 1 && py_int != 1)
233  // we assume px_int and py_int > 0
234  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
235  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
236  u = (double)I_.getWidth() / (2 * px_int);
237  v = (double)I_.getHeight() / (2 * py_int);
238  } else {
239  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
240  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
241  }
242 
243  float o44c[4][4], o44cd[4][4], x, y, z;
244  Matrix id = IDENTITY_MATRIX;
245 
247  get_fMi(fMit);
248  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
249  this->cMo = rotz * cMo;
250 
251  vp2jlc_matrix(cMo.inverse(), o44c);
252  vp2jlc_matrix(cdMo.inverse(), o44cd);
253 
254  while (get_displayBusy())
255  vpTime::wait(2);
256 
257  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
258  x = o44c[2][0] + o44c[3][0];
259  y = o44c[2][1] + o44c[3][1];
260  z = o44c[2][2] + o44c[3][2];
261  add_vwstack("start", "vrp", x, y, z);
262  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
263  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
264  add_vwstack("start", "window", -u, u, -v, v);
265  if (displayObject)
266  display_scene(id, this->scene, I_, curColor);
267 
268  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
269  x = o44cd[2][0] + o44cd[3][0];
270  y = o44cd[2][1] + o44cd[3][1];
271  z = o44cd[2][2] + o44cd[3][2];
272  add_vwstack("start", "vrp", x, y, z);
273  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
274  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
275  add_vwstack("start", "window", -u, u, -v, v);
276  if (displayDesiredObject) {
277  if (desiredObject == D_TOOL)
279  else
281  }
282  delete[] fMit;
283  set_displayBusy(false);
284 }
285 
298 {
299 
300  if (!sceneInitialized)
301  throw;
302 
303  double u;
304  double v;
305  // if(px_int != 1 && py_int != 1)
306  // we assume px_int and py_int > 0
307  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
308  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
309  u = (double)I.getWidth() / (2 * px_int);
310  v = (double)I.getHeight() / (2 * py_int);
311  } else {
312  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
313  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
314  }
315 
316  float o44c[4][4], o44cd[4][4], x, y, z;
317  Matrix id = IDENTITY_MATRIX;
318 
320  get_fMi(fMit);
321  this->cMo = fMit[size_fMi - 1].inverse() * fMo;
322  this->cMo = rotz * cMo;
323 
324  vp2jlc_matrix(cMo.inverse(), o44c);
325  vp2jlc_matrix(cdMo.inverse(), o44cd);
326 
327  while (get_displayBusy())
328  vpTime::wait(2);
329 
330  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
331  x = o44c[2][0] + o44c[3][0];
332  y = o44c[2][1] + o44c[3][1];
333  z = o44c[2][2] + o44c[3][2];
334  add_vwstack("start", "vrp", x, y, z);
335  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
336  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
337  add_vwstack("start", "window", -u, u, -v, v);
338  if (displayObject) {
339  display_scene(id, this->scene, I_, curColor);
340  }
341 
342  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
343  x = o44cd[2][0] + o44cd[3][0];
344  y = o44cd[2][1] + o44cd[3][1];
345  z = o44cd[2][2] + o44cd[3][2];
346  add_vwstack("start", "vrp", x, y, z);
347  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
348  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
349  add_vwstack("start", "window", -u, u, -v, v);
350  if (displayDesiredObject) {
351  if (desiredObject == D_TOOL)
353  else
355  }
356  delete[] fMit;
357  set_displayBusy(false);
358 }
359 
366 {
367  vpHomogeneousMatrix cMoTemp;
369  get_fMi(fMit);
370  cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
371  delete[] fMit;
372  return cMoTemp;
373 }
374 
375 #elif !defined(VISP_BUILD_SHARED_LIBS)
376 // Work arround to avoid warning:
377 // libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
378 void dummy_vpRobotWireFrameSimulator(){};
379 #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:252
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Implementation of an homogeneous matrix and operations on such kind of matrices.
A cylindrical tool is attached to the camera.
vpHomogeneousMatrix cdMo
vpHomogeneousMatrix fMo
static const vpColor red
Definition: vpColor.h:217
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
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)
vpHomogeneousMatrix cMo
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpSceneDesiredObject desiredObject
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:188