Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
vpSimulatorAfma6.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 
39 #ifndef vpSimulatorAfma6_HH
40 #define vpSimulatorAfma6_HH
41 
42 #include <visp3/core/vpConfig.h>
43 #include <visp3/robot/vpAfma6.h>
44 #include <visp3/robot/vpRobotWireFrameSimulator.h>
45 
46 #include <string>
47 
48 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
49 
180 class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
181 {
182 public:
183  static const double defaultPositioningVelocity;
184 
185 private:
186  vpColVector q_prev_getdis;
187  bool first_time_getdis;
188 
189  double positioningVelocity;
190 
191  vpColVector zeroPos;
192  vpColVector reposPos;
193 
194  bool toolCustom;
195  std::string arm_dir;
196 
197 public:
199  VP_EXPLICIT vpSimulatorAfma6(bool display);
200  virtual ~vpSimulatorAfma6() VP_OVERRIDE;
201 
202  void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height);
203  void getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I);
204  void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
205  void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement) VP_OVERRIDE;
206  void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE;
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) VP_OVERRIDE;
219  void get_fJe(vpMatrix &fJe) VP_OVERRIDE;
220 
221  void
224  bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
225  void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
226 
227  void move(const char *filename);
228 
229  static bool readPosFile(const std::string &filename, vpColVector &q);
230  static bool savePosFile(const std::string &filename, const vpColVector &q);
231  void setCameraParameters(const vpCameraParameters &cam);
232  void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax);
233 
234  void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) VP_OVERRIDE;
235  void setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3, double pos4,
236  double pos5, double pos6);
237  void setPosition(const char *filename);
238  void setPositioningVelocity(double vel) { positioningVelocity = vel; }
239  bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint = nullptr, const double &errMax = 0.001);
241 
242  void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) VP_OVERRIDE;
243 
244  void stopMotion();
245 
246 protected:
249  void computeArticularVelocity() VP_OVERRIDE;
250  void compute_fMi();
251  void findHighestPositioningSpeed(vpColVector &q);
252  void getExternalImage(vpImage<vpRGBa> &I);
253  inline void get_fMi(vpHomogeneousMatrix *fMit) VP_OVERRIDE
254  {
255  m_mutex_fMi.lock();
256  for (int i = 0; i < 8; i++) {
257  fMit[i] = fMi[i];
258  }
259 
260  m_mutex_fMi.unlock();
261  }
262  void init() VP_OVERRIDE;
263  void initArms() VP_OVERRIDE;
264  void initDisplay();
265  int isInJointLimit() VP_OVERRIDE;
266  bool singularityTest(const vpColVector &q, vpMatrix &J);
267  void updateArticularPosition() VP_OVERRIDE;
269 };
270 END_VISP_NAMESPACE
271 #endif
272 
273 #endif
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:78
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:920
void init(void)
Definition: vpAfma6.cpp:158
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:898
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:942
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1012
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition: vpImage.h:131
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Definition: vpRGBa.h:65
This class aims to be a basis used to create all the simulators of robots.
virtual void computeArticularVelocity()=0
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
vpControlFrameType
Definition: vpRobot.h:77
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
vpRobotStateType
Definition: vpRobot.h:65
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.
Simulator of Irisa's gantry robot named Afma6.
void setPositioningVelocity(double vel)
static const double defaultPositioningVelocity