iCub-main
DebugInterfaces.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
5  * Authors: Giorgio Metta
6  * email: giorgio.metta@iit.it
7  * website: www.robotcub.org
8  * Permission is granted to copy, distribute, and/or modify this program
9  * under the terms of the GNU General Public License, version 2 or any
10  * later version published by the Free Software Foundation.
11  *
12  * A copy of the license can be found at
13  * http://www.robotcub.org/icub/license/gpl.txt
14  *
15  * This program is distributed in the hope that it will be useful, but
16  * WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18  * Public License for more details
19  */
20 
38 #ifndef __DEBUGINTERFACES__
39 #define __DEBUGINTERFACES__
40 
41 #include <math.h>
42 #include <string.h> // for memset
43 #include <stdlib.h> // for exit
44 #include <stdio.h> // for printf
45 
46 // additional vocabs defined for the IDebug interface.
47 #define VOCAB_GENERIC_PARAMETER yarp::os::createVocab('g','e','n','p')
48 #define VOCAB_DEBUG_PARAMETER yarp::os::createVocab('d','b','g','p')
49 
50 /* LATER: is it likely that some of these would move into iCub::dev namespace? */
51 namespace yarp{
52  namespace dev {
53  class IDebugInterface;
54  class IDebugInterfaceRaw;
56  }
57 }
58 
59 #define _YARP_ASSERT_DEBUG(x) { if (!(x)) { printf("memory allocation failure\n"); /*yarp::os::exit(1);*/ } }
60 
61 template <class T>
62 inline T* allocAndCheckDebug(int size)
63 {
64  T* t = new T[size];
65  _YARP_ASSERT (t != 0);
66  memset(t, 0, sizeof(T) * size);
67  return t;
68 }
69 
70 template <class T>
71 inline void checkAndDestroyDebug(T* &p) {
72  if (p!=0) {
73  delete [] p;
74  p = 0;
75  }
76 }
77 
79 {
80 public:
81  ControlBoardHelper2(int n, const int *aMap, const double *angToEncs, const double *zs, const double *nw, const double *angToRot): zeros(0),
82  signs(0),
83  axisMap(0),
84  invAxisMap(0),
85  angleToEncoders(0),
86  angleToRotor(0),
87  newtonsToSensors(0)
88  {
89  nj=n;
90  alloc(n);
91 
92  memcpy(axisMap, aMap, sizeof(int)*nj);
93 
94  if (zs!=0)
95  memcpy(zeros, zs, sizeof(double)*nj);
96  else
97  memset(zeros, 0, sizeof(double)*nj);
98 
99  if (angToEncs!=0)
100  memcpy(angleToEncoders, angToEncs, sizeof(double)*nj);
101  else
102  memset(angleToEncoders, 0, sizeof(double)*nj);
103 
104  if (angToRot!=0)
105  memcpy(angleToRotor, angToRot, sizeof(double)*nj);
106  else
107  memset(angleToRotor, 0, sizeof(double)*nj);
108 
109  if (nw!=0)
110  memcpy(newtonsToSensors, nw, sizeof(double)*nj);
111  else
112  memset(newtonsToSensors, 0, sizeof(double)*nj);
113 
114  // invert the axis map
115  memset (invAxisMap, 0, sizeof(int) * nj);
116  int i;
117  for (i = 0; i < nj; i++)
118  {
119  int j;
120  for (j = 0; j < nj; j++)
121  {
122  if (axisMap[j] == i)
123  {
124  invAxisMap[i] = j;
125  break;
126  }
127  }
128  }
129 
130  }
131 
133  {
134  dealloc();
135  }
136 
137  bool alloc(int n)
138  {
139  nj=n;
140  if (nj<=0)
141  return false;
142 
143  if (zeros!=0)
144  dealloc();
145 
146  zeros=new double [nj];
147  signs=new double [nj];
148  axisMap=new int [nj];
149  invAxisMap=new int [nj];
150  angleToEncoders=new double [nj];
151  angleToRotor=new double [nj];
152  newtonsToSensors=new double [nj];
153  _YARP_ASSERT_DEBUG(zeros != 0 && signs != 0 && axisMap != 0 && invAxisMap != 0 && angleToEncoders != 0 && newtonsToSensors != 0);
154 
155  return true;
156  }
157 
158  bool dealloc()
159  {
160  checkAndDestroyDebug<double> (zeros);
161  checkAndDestroyDebug<double> (signs);
162  checkAndDestroyDebug<int> (axisMap);
163  checkAndDestroyDebug<int> (invAxisMap);
164  checkAndDestroyDebug<double> (angleToEncoders);
165  checkAndDestroyDebug<double> (angleToRotor);
166  checkAndDestroyDebug<double> (newtonsToSensors);
167  return true;
168  }
169 
170  inline int toHw(int axis)
171  { return axisMap[axis]; }
172 
173  inline int toUser(int axis)
174  { return invAxisMap[axis]; }
175 
176  //map a vector, no conversion
177  inline void toUser(const double *hwData, double *user)
178  {
179  for (int k=0;k<nj;k++)
180  user[toUser(k)]=hwData[k];
181  }
182 
183  //map a vector, no conversion
184  inline void toUser(const int *hwData, int *user)
185  {
186  for (int k=0;k<nj;k++)
187  user[toUser(k)]=hwData[k];
188  }
189 
190  //map a vector, no conversion
191  inline void toHw(const double *usr, double *hwData)
192  {
193  for (int k=0;k<nj;k++)
194  hwData[toHw(k)]=usr[k];
195  }
196 
197  //map a vector, no conversion
198  inline void toHw(const int *usr, int *hwData)
199  {
200  for (int k=0;k<nj;k++)
201  hwData[toHw(k)]=usr[k];
202  }
203 
204  inline void posA2E(double ang, int j, double &enc, int &k)
205  {
206  enc=(ang+zeros[j])*angleToEncoders[j];
207  k=toHw(j);
208  }
209 
210  inline double posA2E(double ang, int j)
211  {
212  return (ang+zeros[j])*angleToEncoders[j];
213  }
214 
215  inline void posE2A(double enc, int j, double &ang, int &k)
216  {
217  k=toUser(j);
218 
219  ang=(enc/angleToEncoders[k])-zeros[k];
220  }
221 
222  inline double posE2A(double enc, int j)
223  {
224  int k=toUser(j);
225 
226  return (enc/angleToEncoders[k])-zeros[k];
227  }
228 
229  inline void posR2A(double enc, int j, double &ang, int &k)
230  {
231  k=toUser(j);
232 
233  ang=(enc/angleToRotor[k]);//-zeros[k];
234  }
235 
236  inline double posR2A(double enc, int j)
237  {
238  int k=toUser(j);
239 
240  return (enc/angleToRotor[k]);//-zeros[k];
241  }
242 
243  inline void impN2S(double newtons, int j, double &sens, int &k)
244  {
245  sens=newtons*newtonsToSensors[j]/angleToEncoders[j];
246  k=toHw(j);
247  }
248 
249  inline double impN2S(double newtons, int j)
250  {
251  return newtons*newtonsToSensors[j]/angleToEncoders[j];
252  }
253 
254  inline void impN2S(const double *newtons, double *sens)
255  {
256  double tmp;
257  int index;
258  for(int j=0;j<nj;j++)
259  {
260  impN2S(newtons[j], j, tmp, index);
261  sens[index]=tmp;
262  }
263  }
264 
265  inline void trqN2S(double newtons, int j, double &sens, int &k)
266  {
267  sens=newtons*newtonsToSensors[j];
268  k=toHw(j);
269  }
270 
271  inline double trqN2S(double newtons, int j)
272  {
273  return newtons*newtonsToSensors[j];
274  }
275 
276  //map a vector, convert from newtons to sensors
277  inline void trqN2S(const double *newtons, double *sens)
278  {
279  double tmp;
280  int index;
281  for(int j=0;j<nj;j++)
282  {
283  trqN2S(newtons[j], j, tmp, index);
284  sens[index]=tmp;
285  }
286  }
287 
288  //map a vector, convert from sensor to newtons
289  inline void trqS2N(const double *sens, double *newtons)
290  {
291  double tmp;
292  int index;
293  for(int j=0;j<nj;j++)
294  {
295  trqS2N(sens[j], j, tmp, index);
296  newtons[index]=tmp;
297  }
298  }
299 
300  inline void trqS2N(double sens, int j, double &newton, int &k)
301  {
302  k=toUser(j);
303 
304  newton=(sens/newtonsToSensors[k]);
305  }
306 
307  inline double trqS2N(double sens, int j)
308  {
309  int k=toUser(j);
310 
311  return (sens/newtonsToSensors[k]);
312  }
313 
314  inline void impS2N(const double *sens, double *newtons)
315  {
316  double tmp;
317  int index;
318  for(int j=0;j<nj;j++)
319  {
320  impS2N(sens[j], j, tmp, index);
321  newtons[index]=tmp;
322  }
323  }
324 
325  inline void impS2N(double sens, int j, double &newton, int &k)
326  {
327  k=toUser(j);
328 
329  newton=(sens/newtonsToSensors[k]*angleToEncoders[k]);
330  }
331 
332  inline double impS2N(double sens, int j)
333  {
334  int k=toUser(j);
335 
336  return (sens/newtonsToSensors[k]*angleToEncoders[k]);
337  }
338 
339  inline void velA2E(double ang, int j, double &enc, int &k)
340  {
341  k=toHw(j);
342  enc=ang*angleToEncoders[j];
343  }
344 
345  inline void velA2E_abs(double ang, int j, double &enc, int &k)
346  {
347  k=toHw(j);
348  enc=ang*fabs(angleToEncoders[j]);
349  }
350 
351  inline void velE2A(double enc, int j, double &ang, int &k)
352  {
353  k=toUser(j);
354  ang=enc/angleToEncoders[k];
355  }
356 
357  inline void velE2A_abs(double enc, int j, double &ang, int &k)
358  {
359  k=toUser(j);
360  ang=enc/fabs(angleToEncoders[k]);
361  }
362 
363  inline void accA2E(double ang, int j, double &enc, int &k)
364  {
365  velA2E(ang, j, enc, k);
366  }
367 
368  inline void accA2E_abs(double ang, int j, double &enc, int &k)
369  {
370  velA2E_abs(ang, j, enc, k);
371  }
372 
373  inline void accE2A(double enc, int j, double &ang, int &k)
374  {
375  velE2A(enc, j, ang, k);
376  }
377 
378  inline void accE2A_abs(double enc, int j, double &ang, int &k)
379  {
380  velE2A_abs(enc, j, ang, k);
381  }
382 
383  inline double velE2A(double enc, int j)
384  {
385  int k=toUser(j);
386  return enc/angleToEncoders[k];
387  }
388 
389  inline double velE2A_abs(double enc, int j)
390  {
391  int k=toUser(j);
392  return enc/fabs(angleToEncoders[k]);
393  }
394 
395 
396  inline double accE2A(double enc, int j)
397  {
398  return velE2A(enc, j);
399  }
400 
401  inline double accE2A_abs(double enc, int j)
402  {
403  return velE2A_abs(enc, j);
404  }
405 
406  //map a vector, convert from angles to encoders
407  inline void posA2E(const double *ang, double *enc)
408  {
409  double tmp;
410  int index;
411  for(int j=0;j<nj;j++)
412  {
413  posA2E(ang[j], j, tmp, index);
414  enc[index]=tmp;
415  }
416  }
417 
418  //map a vector, convert from encoders to angles
419  inline void posE2A(const double *enc, double *ang)
420  {
421  double tmp;
422  int index;
423  for(int j=0;j<nj;j++)
424  {
425  posE2A(enc[j], j, tmp, index);
426  ang[index]=tmp;
427  }
428  }
429 
430  inline void velA2E(const double *ang, double *enc)
431  {
432  double tmp;
433  int index;
434  for(int j=0;j<nj;j++)
435  {
436  velA2E(ang[j], j, tmp, index);
437  enc[index]=tmp;
438  }
439  }
440 
441  inline void velA2E_abs(const double *ang, double *enc)
442  {
443  double tmp;
444  int index;
445  for(int j=0;j<nj;j++)
446  {
447  velA2E_abs(ang[j], j, tmp, index);
448  enc[index]=tmp;
449  }
450  }
451 
452  inline void velE2A(const double *enc, double *ang)
453  {
454  double tmp;
455  int index;
456  for(int j=0;j<nj;j++)
457  {
458  velE2A(enc[j], j, tmp, index);
459  ang[index]=tmp;
460  }
461  }
462 
463  inline void velE2A_abs(const double *enc, double *ang)
464  {
465  double tmp;
466  int index;
467  for(int j=0;j<nj;j++)
468  {
469  velE2A_abs(enc[j], j, tmp, index);
470  ang[index]=tmp;
471  }
472  }
473 
474  inline void accA2E(const double *ang, double *enc)
475  {
476  double tmp;
477  int index;
478  for(int j=0;j<nj;j++)
479  {
480  accA2E(ang[j], j, tmp, index);
481  enc[index]=tmp;
482  }
483  }
484 
485  inline void accA2E_abs(const double *ang, double *enc)
486  {
487  double tmp;
488  int index;
489  for(int j=0;j<nj;j++)
490  {
491  accA2E_abs(ang[j], j, tmp, index);
492  enc[index]=tmp;
493  }
494  }
495 
496  inline void accE2A(const double *enc, double *ang)
497  {
498  double tmp;
499  int index;
500  for(int j=0;j<nj;j++)
501  {
502  accE2A(enc[j], j, tmp, index);
503  ang[index]=tmp;
504  }
505  }
506 
507  inline void accE2A_abs(const double *enc, double *ang)
508  {
509  double tmp;
510  int index;
511  for(int j=0;j<nj;j++)
512  {
513  accE2A_abs(enc[j], j, tmp, index);
514  ang[index]=tmp;
515  }
516  }
517 
518  inline int axes()
519  { return nj; }
520 
521  int nj;
522 
523  double *zeros;
524  double *signs;
525  int *axisMap;
528  double *angleToRotor;
530 };
531 
533 { return static_cast<ControlBoardHelper2 *>(p); }
534 
535 
536 inline ControlBoardHelper2 *castToMapper2(void *p);
537 
538 
545 public:
546  virtual ~IDebugInterface() {}
547 
548  /* Set a generic parameter (for debug)
549  * @param type is the CAN code representing the command message
550  * @return true/false on success/failure
551  */
552  virtual bool setParameter(int j, unsigned int type, double value)=0;
553 
554  /* Get a generic parameter (for debug)
555  * @param type is the CAN code representing the command message
556  * @return true/false on success/failure
557  */
558  virtual bool getParameter(int j, unsigned int type, double* value)=0;
559 
560  /* Set a generic parameter (for debug)
561  * @param index is the number of the debug parameter
562  * @return true/false on success/failure
563  */
564  virtual bool setDebugParameter(int j, unsigned int index, double value)=0;
565 
566  /* Get a generic parameter (for debug)
567  * @param index is the number of the debug parameter
568  * @return true/false on success/failure
569  */
570  virtual bool getDebugParameter(int j, unsigned int index, double* value)=0;
571 };
572 
574 public:
575  virtual ~IDebugInterfaceRaw() {}
576 
577  /* Set a generic parameter (for debug)
578  * @param type is the CAN code representing the command message
579  * @return true/false on success/failure
580  */
581  virtual bool setParameterRaw(int j, unsigned int type, double value)=0;
582 
583  /* Get a generic parameter (for debug)
584  * @param type is the CAN code representing the command message
585  * @return true/false on success/failure
586  */
587  virtual bool getParameterRaw(int j, unsigned int type, double* value)=0;
588 
589  /* Set a generic parameter (for debug)
590  * @param index is the number of the debug parameter
591  * @return true/false on success/failure
592  */
593  virtual bool setDebugParameterRaw(int j, unsigned int index, double value)=0;
594 
595  /* Get a generic parameter (for debug)
596  * @param index is the number of the debug parameter
597  * @return true/false on success/failure
598  */
599  virtual bool getDebugParameterRaw(int j, unsigned int index, double* value)=0;
600 };
601 
603 {
604  void *helper;
606  double *dummy;
607 public:
608  bool initialize(int k, const int *amap, const double *angleToEncoder, const double *zeros, const double *rotToEncoder);
609  bool uninitialize();
612  bool setParameter (int j, unsigned int type, double value);
613  bool getParameter (int j, unsigned int type, double* value);
614  bool setDebugParameter (int j, unsigned int index, double value);
615  bool getDebugParameter (int j, unsigned int index, double *value);
616 };
617 
618 #endif /* __DEBUGINTERFACES__ */
void toUser(const int *hwData, int *user)
#define _YARP_ASSERT_DEBUG(x)
void trqN2S(double newtons, int j, double &sens, int &k)
void trqN2S(const double *newtons, double *sens)
double trqN2S(double newtons, int j)
double accE2A_abs(double enc, int j)
int toUser(int axis)
void velA2E(double ang, int j, double &enc, int &k)
void posE2A(double enc, int j, double &ang, int &k)
void trqS2N(double sens, int j, double &newton, int &k)
void checkAndDestroyDebug(T *&p)
double velE2A(double enc, int j)
double posA2E(double ang, int j)
zeros(2, 2) eye(2
void accA2E(double ang, int j, double &enc, int &k)
T * allocAndCheckDebug(int size)
double accE2A(double enc, int j)
void posA2E(double ang, int j, double &enc, int &k)
double impS2N(double sens, int j)
int n
ControlBoardHelper2 * castToMapper2(void *p)
Copyright (C) 2008 RobotCub Consortium.
void impS2N(double sens, int j, double &newton, int &k)
static int v
Definition: iCub_Sim.cpp:47
double impN2S(double newtons, int j)
void posE2A(const double *enc, double *ang)
ControlBoardHelper2(int n, const int *aMap, const double *angToEncs, const double *zs, const double *nw, const double *angToRot)
void toHw(const double *usr, double *hwData)
void velA2E_abs(double ang, int j, double &enc, int &k)
double posR2A(double enc, int j)
void accE2A(const double *enc, double *ang)
void posA2E(const double *ang, double *enc)
double trqS2N(double sens, int j)
void trqS2N(const double *sens, double *newtons)
void velE2A_abs(const double *enc, double *ang)
void impS2N(const double *sens, double *newtons)
void velE2A(const double *enc, double *ang)
void accE2A_abs(const double *enc, double *ang)
void velA2E_abs(const double *ang, double *enc)
void toHw(const int *usr, int *hwData)
void velA2E(const double *ang, double *enc)
double velE2A_abs(double enc, int j)
void accE2A_abs(double enc, int j, double &ang, int &k)
void velE2A(double enc, int j, double &ang, int &k)
void accA2E(const double *ang, double *enc)
void accA2E_abs(const double *ang, double *enc)
void velE2A_abs(double enc, int j, double &ang, int &k)
void accA2E_abs(double ang, int j, double &enc, int &k)
void posR2A(double enc, int j, double &ang, int &k)
void toUser(const double *hwData, double *user)
void accE2A(double enc, int j, double &ang, int &k)
void impN2S(double newtons, int j, double &sens, int &k)
double posE2A(double enc, int j)
void impN2S(const double *newtons, double *sens)