Visual Servoing Platform  version 3.0.0
vpSimulatorAfma6.h
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Class which provides a simulator for the robot Afma6.
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 #ifndef vpSimulatorAfma6_HH
39 #define vpSimulatorAfma6_HH
40 
46 #include <visp3/robot/vpRobotWireFrameSimulator.h>
47 #include <visp3/robot/vpAfma6.h>
48 
49 #include <string>
50 
51 #if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
52 
176 class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
177 {
178  public:
179  static const double defaultPositioningVelocity;
180 
181  private:
182  vpColVector q_prev_getdis;
183  bool first_time_getdis;
184 
185  double positioningVelocity;
186 
187  vpColVector zeroPos;
188  vpColVector reposPos;
189 
190  bool toolCustom;
191  std::string arm_dir;
192 
193 public:
195  vpSimulatorAfma6(bool display);
196  virtual ~vpSimulatorAfma6();
197 
199  const unsigned int &image_width,
200  const unsigned int &image_height);
202  const vpImage<unsigned char> &I);
205  vpColVector &displacement);
207  void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
208  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
209  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp);
210  double getPositioningVelocity (void){return positioningVelocity;}
211  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
212  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
213  vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
214  vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double &timestamp);
215 
216  void get_cMe(vpHomogeneousMatrix &cMe);
217  void get_cVe(vpVelocityTwistMatrix &cVe);
218  void get_eJe(vpMatrix &eJe);
219  void get_fJe(vpMatrix &fJe);
220 
222  bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
223  void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
224 
225  void move(const char *filename) ;
226 
227  static bool readPosFile(const char *filename, vpColVector &q);
228  static bool savePosFile(const char *filename, const vpColVector &q);
229  void setCameraParameters(const vpCameraParameters &cam) ;
230  void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
231 
232  void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
233  void setPosition (const vpRobot::vpControlFrameType frame,
234  const double pos1,
235  const double pos2,
236  const double pos3,
237  const double pos4,
238  const double pos5,
239  const double pos6);
240  void setPosition(const char *filename);
241  void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
242  bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
244 
245  void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
246 
247  void stopMotion();
248 
249 
250 protected:
252  void compute_fMi();
253  void findHighestPositioningSpeed(vpColVector &q);
255  inline void get_fMi(vpHomogeneousMatrix *fMit) {
256 #if defined(_WIN32)
257  WaitForSingleObject(mutex_fMi,INFINITE);
258  for (int i = 0; i < 8; i++)
259  fMit[i] = fMi[i];
260  ReleaseMutex(mutex_fMi);
261 #elif defined(VISP_HAVE_PTHREAD)
262  pthread_mutex_lock (&mutex_fMi);
263  for (int i = 0; i < 8; i++)
264  fMit[i] = fMi[i];
265  pthread_mutex_unlock (&mutex_fMi);
266 #endif
267  }
268  void init();
269  void initArms();
270  void initDisplay();
271  int isInJointLimit (void);
272  bool singularityTest(const vpColVector q, vpMatrix &J);
274 
275 private:
276  void getCameraDisplacement(vpColVector &displacement);
277  void getArticularDisplacement(vpColVector &displacement);
278 };
279 
280 #endif
281 
282 #endif
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:65
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1244
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:872
void setPositioningVelocity(const double vel)
virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Get a displacement (frame as to ve specified) between two successive position control.
Perspective projection without distortion model.
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void initArms()=0
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:895
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:104
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.
static const double defaultPositioningVelocity
vpControlFrameType
Definition: vpRobot.h:76
virtual int isInJointLimit()=0
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpRobotStateType
Definition: vpRobot.h:64
virtual void updateArticularPosition()=0
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:857
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Get the robot position (frame has to be specified).
virtual void computeArticularVelocity()=0
Generic class defining intrinsic camera parameters.
Simulator of Irisa's gantry robot named Afma6.
Implementation of a velocity twist matrix and operations on such kind of matrices.
void getExternalImage(vpImage< unsigned char > &I)
This class aims to be a basis used to create all the simulators of robots.
double getPositioningVelocity(void)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
void init(void)
Definition: vpAfma6.cpp:195
void get_fMi(vpHomogeneousMatrix *fMit)
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
Set the velocity (frame has to be specified) that will be applied to the velocity controller...
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:964