ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpSimulatorViper850.h
1 /****************************************************************************
2  *
3  * $Id: vpSimulatorViper850.h 4252 2013-05-14 13:44:49Z 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  * Class which provides a simulator for the robot Viper850.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 #ifndef vpSimulatorViper850_HH
43 #define vpSimulatorViper850_HH
44 
52 #include <visp/vpRobotWireFrameSimulator.h>
53 #include <visp/vpViper850.h>
54 #include <string>
55 #if defined(WIN32) || defined(VISP_HAVE_PTHREAD)
56 
206 class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, public vpViper850
207 {
208  public:
209  static const double defaultPositioningVelocity;
210 
211  private:
212  vpColVector q_prev_getdis;
213  bool first_time_getdis;
214 
215  double positioningVelocity;
216 
217  vpColVector zeroPos;
218  vpColVector reposPos;
219 
220  bool toolCustom;
221  std::string arm_dir;
222 
223  public:
225  vpSimulatorViper850(bool display);
226  virtual ~vpSimulatorViper850();
227 
229  const unsigned int &image_width,
230  const unsigned int &image_height);
232  const vpImage<unsigned char> &I);
234 
236  vpColVector &displacement);
237 
239  void getPosition (const vpRobot::vpControlFrameType frame,
240  vpPoseVector &position);
241  double getPositioningVelocity (void){return positioningVelocity;}
242 
243 
244  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
245  vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
246 
247  void get_cMe(vpHomogeneousMatrix &cMe);
248  void get_cVe(vpVelocityTwistMatrix &cVe);
249  void get_eJe(vpMatrix &eJe);
250  void get_fJe(vpMatrix &fJe);
251 
253  bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
254  void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
255 
256  void move(const char *filename) ;
257 
258  static bool readPosFile(const char *filename, vpColVector &q);
259  static bool savePosFile(const char *filename, const vpColVector &q);
260 
261  void setCameraParameters(const vpCameraParameters &cam) ;
262  void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
263  void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
264  void setPosition (const vpRobot::vpControlFrameType frame,
265  const double pos1,
266  const double pos2,
267  const double pos3,
268  const double pos4,
269  const double pos5,
270  const double pos6);
271  void setPosition(const char *filename);
272  void setPositioningVelocity (const double velocity) {positioningVelocity = velocity;}
274 
275  void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
276 
277  void stopMotion();
278 
279 protected:
281  void compute_fMi();
282  void findHighestPositioningSpeed(vpColVector &q);
284 
285  inline void get_fMi(vpHomogeneousMatrix *fMit) {
286 #if defined(WIN32)
287  WaitForSingleObject(mutex_fMi,INFINITE);
288  for (int i = 0; i < 8; i++)
289  fMit[i] = fMi[i];
290  ReleaseMutex(mutex_fMi);
291 #elif defined(VISP_HAVE_PTHREAD)
292  pthread_mutex_lock (&mutex_fMi);
293  for (int i = 0; i < 8; i++)
294  fMit[i] = fMi[i];
295  pthread_mutex_unlock (&mutex_fMi);
296 #endif
297  }
298  void init();
299  void initArms();
300  void initDisplay();
301  int isInJointLimit (void);
302  bool singularityTest(const vpColVector q, vpMatrix &J);
304 
305 private:
306  void getArticularDisplacement(vpColVector &displacement);
307  void getCameraDisplacement(vpColVector &displacement);
308 };
309 
310 #endif
311 
312 #endif