Visual Servoing Platform  version 3.6.1 under development (2024-05-20)
vpRobotPololuPtu.cpp
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  * Common features for Pololu Maestro PanTiltUnit.
32  */
33 
34 #include <visp3/core/vpConfig.h>
35 
36 #if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_THREADS)
37 
38 #include <visp3/core/vpDebug.h>
39 #include <visp3/robot/vpRobot.h>
40 #include <visp3/robot/vpRobotException.h>
41 #include <visp3/robot/vpRobotPololuPtu.h>
42 
43 vpRobotPololuPtu::vpRobotPololuPtu(const std::string &device, int baudrate, bool verbose)
44  : m_verbose(verbose)
45 {
46  nDof = 2;
47  m_pan.connect(device, baudrate, 0);
48  m_pan.setPwmRange(4095, 7905);
49  m_pan.setAngularRange(static_cast<float>(vpMath::rad(-45)), static_cast<float>(vpMath::rad(45)));
50  m_pan.setVerbose(verbose);
51 
52  m_tilt.connect(device, baudrate, 1);
53  m_tilt.setPwmRange(4095, 7905);
54  m_tilt.setAngularRange(static_cast<float>(vpMath::rad(-45)), static_cast<float>(vpMath::rad(45)));
55  m_tilt.setVerbose(verbose);
56 }
57 
59 {
60  m_pan.stopVelocityCmd();
61  m_tilt.stopVelocityCmd();
62 }
63 
65 {
66  vpColVector q(nDof);
68 
69  get_eJe(q, eJe);
70 }
71 
73 {
74  vpColVector q(nDof);
76 
77  get_fJe(q, fJe);
78 }
79 
81 {
82  eJe.resize(6, nDof);
83 
84  if (q.size() != static_cast<unsigned int>(nDof)) {
85  throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector"));
86  }
87 
88  double s2 = sin(q[1]);
89  double c2 = cos(q[1]);
90 
91  eJe = 0;
92 
93  eJe[3][0] = -c2;
94  eJe[4][1] = -1;
95  eJe[5][0] = s2;
96 }
97 
99 {
100  if (q.size() != static_cast<unsigned int>(nDof)) {
101  throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector"));
102  }
103 
104  fJe.resize(6, nDof);
105 
106  double s1 = sin(q[0]);
107  double c1 = cos(q[0]);
108 
109  fJe = 0;
110 
111  fJe[3][1] = s1;
112  fJe[4][1] = -c1;
113  fJe[5][0] = 1;
114 }
115 
117 {
118  return m_pan.speedToRadS(1);
119 }
120 
122 {
123  if (q.size() != static_cast<unsigned int>(nDof)) {
124  throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector"));
125  }
126 
128  std::cout << "Warning: Robot is not in position-based control. Modification of the robot state" << std::endl;
130  }
131 
132  switch (frame) {
134  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
135  "not implemented");
136  break;
138  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
139  "not implemented");
140  break;
141  case vpRobot::MIXT_FRAME:
142  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
143  "not implemented");
144  break;
146  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
147  "not implemented");
148  break;
150  break;
151  }
152 
153  float pos_vel = m_positioning_velocity_percentage * static_cast<float>(getMaxRotationVelocity());
154  m_pan.setAngularPosition(static_cast<float>(q[0]), pos_vel);
155  m_tilt.setAngularPosition(static_cast<float>(q[1]), pos_vel);
156 }
157 
159 {
160  if (q_dot.size() != static_cast<unsigned int>(nDof)) {
161  throw(vpException(vpException::dimensionError, "Bad dimension for Pololu PTU joint position vector"));
162  }
163 
166  "Cannot send a velocity to the robot "
167  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
168  }
169 
170  switch (frame) {
171  case vpRobot::CAMERA_FRAME: {
172  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
173  "in the camera frame:"
174  "functionality not implemented");
175  }
176  case vpRobot::JOINT_STATE: {
177  if (q_dot.getRows() != 2) {
178  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
179  "in joint state");
180  }
181  break;
182  }
184  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
185  "in the reference frame:"
186  "functionality not implemented");
187  }
188  case vpRobot::MIXT_FRAME: {
189  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
190  "in the mixt frame:"
191  "functionality not implemented");
192  }
194  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
195  "in the end-effector frame:"
196  "functionality not implemented");
197  }
198  default: {
199  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
200  }
201  }
202 
203  bool norm = false; // Flag to indicate when velocities need to be normalized
204 
205  // Saturate joint speed
206  double max = getMaxRotationVelocity();
207  vpColVector q_dot_sat(nDof);
208 
209  // Init q_dot_saturated
210  q_dot_sat = q_dot;
211 
212  for (unsigned int i = 0; i < static_cast<unsigned int>(nDof); ++i) { // q1 and q2
213  if (fabs(q_dot[i]) > max) {
214  norm = true;
215  max = fabs(q_dot[i]);
216  vpERROR_TRACE("Excess velocity: ROTATION "
217  "(axe nr.%d).",
218  i);
219  }
220  }
221  // Rotations velocities normalization
222  if (norm == true) {
223  max = getMaxRotationVelocity() / max;
224  q_dot_sat = q_dot * max;
225  }
226 
227  std::cout << "Send velocity: " << q_dot_sat.t() << std::endl;
228 
229  m_pan.setAngularVelocity(static_cast<float>(q_dot_sat[0]));
230  m_tilt.setAngularVelocity(static_cast<float>(q_dot_sat[1]));
231 }
232 
234 {
235  switch (newState) {
236  case vpRobot::STATE_STOP: {
238  stopVelocity();
239  }
240  break;
241  }
244  std::cout << "Switch from velocity to position control." << std::endl;
245  stopVelocity();
246  }
247 
248  break;
249  }
251  }
252  default:
253  break;
254  }
255 
256  return vpRobot::setRobotState(newState);
257 }
258 
260 {
261  m_pan.stopVelocityCmd();
262  m_tilt.stopVelocityCmd();
263 }
264 
266 {
267  switch (frame) {
269  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
270  "not implemented");
271  break;
273  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
274  "not implemented");
275  break;
276  case vpRobot::MIXT_FRAME:
277  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
278  "not implemented");
279  break;
281  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
282  "not implemented");
283  break;
285  break;
286  }
287 
288  q.resize(nDof);
289  q[0] = m_pan.getAngularPosition();
290  q[1] = m_tilt.getAngularPosition();
291 }
292 
293 #elif !defined(VISP_BUILD_SHARED_LIBS)
294 // Work around to avoid warning: libvisp_robot.a(vpRobotPololuPtu.cpp.o) has no symbols
295 void dummy_vpRobotPololuPtu() { };
296 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:354
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
unsigned int getRows() const
Definition: vpArray2D.h:339
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
static double rad(double deg)
Definition: vpMath.h:127
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void setVerbose(bool verbose)
Definition: vpPololu.h:238
float speedToRadS(short speed) const
Definition: vpPololu.cpp:282
void setAngularVelocity(float vel_rad_s)
Definition: vpPololu.cpp:238
void setAngularRange(float min_angle, float max_angle)
Definition: vpPololu.h:183
void setPwmRange(unsigned short min_pwm, unsigned short max_pwm)
Definition: vpPololu.h:217
void connect(const std::string &device, int baudrate, int channel)
Definition: vpPololu.cpp:65
float getAngularPosition() const
Definition: vpPololu.cpp:165
void setAngularPosition(float pos_rad, float vel_rad_s=0.f)
Definition: vpPololu.cpp:193
void stopVelocityCmd()
Definition: vpPololu.cpp:287
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
~vpRobotPololuPtu() vp_override
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) vp_override
void get_eJe(vpMatrix &eJe) vp_override
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
vpRobotPololuPtu(const std::string &device="/dev/ttyACM0", int baudrate=9600, bool verbose=false)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
float getAngularVelocityResolution() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) vp_override
void get_fJe(vpMatrix &fJe) vp_override
int nDof
number of degrees of freedom
Definition: vpRobot.h:102
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
#define vpERROR_TRACE
Definition: vpDebug.h:384