ViSP  2.9.0
vpSimulatorAfma6.h
1 /****************************************************************************
2  *
3  * $Id: vpSimulatorAfma6.h 2598 2010-06-02 09:20:22Z nmelchio $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 Afma6.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 #ifndef vpSimulatorAfma6_HH
43 #define vpSimulatorAfma6_HH
44 
50 #include <visp/vpRobotWireFrameSimulator.h>
51 #include <visp/vpAfma6.h>
52 
53 #include <string>
54 
55 #if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
56 
181 class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
182 {
183  public:
184  static const double defaultPositioningVelocity;
185 
186  private:
187  vpColVector q_prev_getdis;
188  bool first_time_getdis;
189 
190  double positioningVelocity;
191 
192  vpColVector zeroPos;
193  vpColVector reposPos;
194 
195  bool toolCustom;
196  std::string arm_dir;
197 
198 public:
200  vpSimulatorAfma6(bool display);
201  virtual ~vpSimulatorAfma6();
202 
204  const unsigned int &image_width,
205  const unsigned int &image_height);
207  const vpImage<unsigned char> &I);
210  vpColVector &displacement);
212  void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
213  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
214  void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp);
215  double getPositioningVelocity (void){return positioningVelocity;}
216  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
217  void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp);
218  vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
219  vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double &timestamp);
220 
221  void get_cMe(vpHomogeneousMatrix &cMe);
222  void get_cVe(vpVelocityTwistMatrix &cVe);
223  void get_eJe(vpMatrix &eJe);
224  void get_fJe(vpMatrix &fJe);
225 
227  bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
228  void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
229 
230  void move(const char *filename) ;
231 
232  static bool readPosFile(const char *filename, vpColVector &q);
233  static bool savePosFile(const char *filename, const vpColVector &q);
234  void setCameraParameters(const vpCameraParameters &cam) ;
235  void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
236 
237  void setPosition(const vpRobot::vpControlFrameType frame,const vpColVector &q);
238  void setPosition (const vpRobot::vpControlFrameType frame,
239  const double pos1,
240  const double pos2,
241  const double pos3,
242  const double pos4,
243  const double pos5,
244  const double pos6);
245  void setPosition(const char *filename);
246  void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
247  bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
249 
250  void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
251 
252  void stopMotion();
253 
254 
255 protected:
257  void compute_fMi();
258  void findHighestPositioningSpeed(vpColVector &q);
260  inline void get_fMi(vpHomogeneousMatrix *fMit) {
261 #if defined(_WIN32)
262  WaitForSingleObject(mutex_fMi,INFINITE);
263  for (int i = 0; i < 8; i++)
264  fMit[i] = fMi[i];
265  ReleaseMutex(mutex_fMi);
266 #elif defined(VISP_HAVE_PTHREAD)
267  pthread_mutex_lock (&mutex_fMi);
268  for (int i = 0; i < 8; i++)
269  fMit[i] = fMi[i];
270  pthread_mutex_unlock (&mutex_fMi);
271 #endif
272  }
273  void init();
274  void initArms();
275  void initDisplay();
276  int isInJointLimit (void);
277  bool singularityTest(const vpColVector q, vpMatrix &J);
279 
280 private:
281  void getCameraDisplacement(vpColVector &displacement);
282  void getArticularDisplacement(vpColVector &displacement);
283 };
284 
285 #endif
286 
287 #endif
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:69
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1248
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:876
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.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
virtual void initArms()=0
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:899
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:108
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:78
virtual int isInJointLimit()=0
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:190
vpRobotStateType
Definition: vpRobot.h:66
virtual void updateArticularPosition()=0
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:861
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.
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void getExternalImage(vpImage< unsigned char > &I)
This class aims to be a basis used to create all the simulators of robots.
double getPositioningVelocity(void)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
void init(void)
Definition: vpAfma6.cpp:199
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:968