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 * Interface for the Irisa's Afma6 robot. 33 * 34 * Authors: 35 * Fabien Spindler 36 * 37 *****************************************************************************/ 38 39 #ifndef _vpAfma6_h 40 #define _vpAfma6_h 41 42 /*! 43 44 \file vpAfma6.h 45 46 Modelisation of Irisa's gantry robot named Afma6. 47 48 */ 49 50 /*! 51 52 \class vpAfma6 53 54 \ingroup group_robot_real_gantry group_robot_simu_gantry 55 56 \brief Modelisation of Irisa's gantry robot named Afma6. 57 58 In this modelisation, different frames have to be considered. 59 60 - \f$ {\cal F}_f \f$: the reference frame, also called world frame 61 62 - \f$ {\cal F}_e \f$: the end-effector frame located at the intersection of 63 the 3 rotations. 64 65 - \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf 66 M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of 67 a calibration stage. We can also consider a custom tool TOOL_CUSTOM and 68 set this tool during robot initialisation or using set_eMc(). 69 70 */ 71 72 #include <visp3/core/vpCameraParameters.h> 73 #include <visp3/core/vpHomogeneousMatrix.h> 74 #include <visp3/core/vpImage.h> 75 #include <visp3/core/vpRGBa.h> 76 #include <visp3/core/vpVelocityTwistMatrix.h> 77 78 class VISP_EXPORT vpAfma6 79 { 80 public: 81 #ifdef VISP_HAVE_AFMA6_DATA 82 //! File where constant parameters in relation with the robot are stored: 83 //! joint max, min, coupling factor between 4 ant 5 joint, distance between 84 //! 5 and 6 joint, tranformation eMc between end-effector and camera frame. 85 static const std::string CONST_AFMA6_FILENAME; 86 static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME; 87 static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME; 88 static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME; 89 static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME; 90 static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME; 91 static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME; 92 static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME; 93 static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME; 94 static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME; 95 static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME; 96 static const std::string CONST_CAMERA_AFMA6_FILENAME; 97 #endif 98 /*! 99 Name of the camera attached to the CCMOP tool 100 (vpAfma6ToolType::TOOL_CCMOP). 101 */ 102 static const char *const CONST_CCMOP_CAMERA_NAME; 103 /*! 104 Name of the camera attached to the 2 fingers gripper tool 105 (vpAfma6ToolType::TOOL_GRIPPER). 106 */ 107 static const char *const CONST_GRIPPER_CAMERA_NAME; 108 /*! 109 Name of the camera attached to the vacuum gripper tool 110 (vpAfma6ToolType::TOOL_VACUUM). 111 */ 112 static const char *const CONST_VACUUM_CAMERA_NAME; 113 /*! 114 Name of the generic camera attached to the robot hand 115 (vpAfma6ToolType::TOOL_GENERIC_CAMERA). 116 */ 117 static const char *const CONST_GENERIC_CAMERA_NAME; 118 119 /*! 120 Name of the Intel D435 camera attached to the robot hand 121 (vpAfma6ToolType::TOOL_INTEL_D435_CAMERA). 122 */ 123 static const char *const CONST_INTEL_D435_CAMERA_NAME; 124 125 //! List of possible tools that can be attached to the robot end-effector. 126 typedef enum { 127 TOOL_CCMOP, /*!< Pneumatic CCMOP gripper. */ 128 TOOL_GRIPPER, /*!< Pneumatic gripper with 2 fingers. */ 129 TOOL_VACUUM, /*!< Pneumatic vaccum gripper. */ 130 TOOL_GENERIC_CAMERA, /*!< A generic camera. */ 131 TOOL_INTEL_D435_CAMERA, /*!< Intel D435 camera */ 132 TOOL_CUSTOM /*!< A user defined tool. */ 133 } vpAfma6ToolType; 134 135 //! Default tool attached to the robot end effector 136 static const vpAfma6ToolType defaultTool; 137 138 public: 139 vpAfma6(); 140 /*! Destructor that does nothing. */ ~vpAfma6()141 virtual ~vpAfma6(){}; 142 143 /** @name Inherited functionalities from vpAfma6 */ 144 //@{ 145 void init(void); 146 void init(const std::string &camera_extrinsic_parameters); 147 void init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters); 148 void init(vpAfma6::vpAfma6ToolType tool, const std::string &filename); 149 void init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc_); 150 void 151 init(vpAfma6::vpAfma6ToolType tool, 152 vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion); 153 154 vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const; 155 int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest = true, 156 const bool &verbose = false) const; 157 158 vpHomogeneousMatrix get_eMc() const; 159 vpHomogeneousMatrix get_fMc(const vpColVector &q) const; 160 void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const; 161 void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const; 162 163 void get_cMe(vpHomogeneousMatrix &cMe) const; 164 void get_cVe(vpVelocityTwistMatrix &cVe) const; 165 void get_eJe(const vpColVector &q, vpMatrix &eJe) const; 166 void get_fJe(const vpColVector &q, vpMatrix &fJe) const; 167 168 //! Get the current tool type getToolType()169 vpAfma6ToolType getToolType() const { return tool_current; }; 170 //! Get the current camera model projection type getCameraParametersProjType()171 vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const { return projModel; }; 172 173 void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, 174 const unsigned int &image_height) const; 175 void getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const; 176 void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const; 177 178 vpColVector getJointMin() const; 179 vpColVector getJointMax() const; 180 double getCoupl56() const; 181 double getLong56() const; 182 183 void parseConfigFile(const std::string &filename); 184 185 virtual void set_eMc(const vpHomogeneousMatrix &eMc); 186 //@} 187 188 friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6); 189 190 protected: 191 /** @name Protected Member Functions Inherited from vpAfma6 */ 192 //@{ 193 //! Set the current tool type setToolType(vpAfma6::vpAfma6ToolType tool)194 void setToolType(vpAfma6::vpAfma6ToolType tool) { tool_current = tool; }; 195 //@} 196 197 public: 198 static const unsigned int njoint; ///< Number of joint. 199 200 protected: 201 double _coupl_56; // coupling between join 5 and 6 202 double _long_56; // distance between join 5 and 6 203 double _joint_max[6]; // Maximal value of the joints 204 double _joint_min[6]; // Minimal value of the joints 205 // Minimal representation of _eMc 206 vpTranslationVector _etc; // meters 207 vpRxyzVector _erc; // radian 208 209 vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera 210 211 protected: 212 //! Current tool in use 213 vpAfma6ToolType tool_current; 214 // Used projection model 215 vpCameraParameters::vpCameraParametersProjType projModel; 216 }; 217 218 #endif 219