sdh.cpp
Go to the documentation of this file.
1 //======================================================================
30 //======================================================================
31 
32 #define _USE_MATH_DEFINES
33 #include <math.h>
34 #include "sdhlibrary_settings.h"
35 
36 //----------------------------------------------------------------------
37 // System Includes - include with <>
38 //----------------------------------------------------------------------
39 
40 #include <assert.h>
41 #include <vector>
42 #include <math.h>
43 #if SDH_USE_VCC
44 # include <float.h>
45 #endif
46 
47 //----------------------------------------------------------------------
48 // Project Includes - include with ""
49 //----------------------------------------------------------------------
50 
51 #include "sdh.h"
52 #include "util.h"
53 #include "release.h"
54 #include "simpletime.h"
55 #if SDH_USE_VCC
56 # include "rs232-vcc.h"
57 #else
58 # include "rs232-cygwin.h"
59 #endif
60 #if WITH_ESD_CAN
61 # include "canserial-esd.h"
62 #endif
63 
64 #if WITH_PEAK_CAN
65 # include "canserial-peak.h"
66 #endif
67 #include "tcpserial.h"
68 
69 //----------------------------------------------------------------------
70 // Defines, enums, unions, structs
71 //----------------------------------------------------------------------
72 
74 
75 
76 //----------------------------------------------------------------------
77 // Global variables (declarations)
78 //----------------------------------------------------------------------
79 
80 
81 //----------------------------------------------------------------------
82 // External functions and classes (declarations)
83 //----------------------------------------------------------------------
84 
85 
86 //----------------------------------------------------------------------
87 // Function definitions
88 //----------------------------------------------------------------------
89 
90 
91 //----------------------------------------------------------------------
92 // Class member definitions
93 //----------------------------------------------------------------------
94 
95 
96 
97 cUnitConverter const cSDH::uc_angle_degrees( "angle", "degrees", "deg", 1.0, 0.0, 1 );
98 
99 cUnitConverter const cSDH::uc_angle_radians( "angle", "radians", "rad", (2.0*M_PI)/360.0, 0.0, 3 );
100 
101 cUnitConverter const cSDH::uc_time_seconds( "time", "seconds", "s", 1.0, 0.0, 3 );
102 
103 cUnitConverter const cSDH::uc_time_milliseconds( "time", "milliseconds", "ms", 1000.0, 0.0, 0 );
104 
105 cUnitConverter const cSDH::uc_temperature_celsius( "temparature", "degrees celsius", "deg C", 1.0, 0.0, 1 );
106 
107 cUnitConverter const cSDH::uc_temperature_fahrenheit( "temparature", "degrees fahrenheit", "deg F", 1.8, 32.0, 1 );
108 
109 cUnitConverter const cSDH::uc_angular_velocity_degrees_per_second( "angular velocity", "degrees/second", "deg/s", 1.0, 0.0, 1 );
110 
111 cUnitConverter const cSDH::uc_angular_velocity_radians_per_second( "angular velocity", "radians/second", "rad/s", (2.0*M_PI)/360.0, 0.0, 3 );
112 
113 cUnitConverter const cSDH::uc_angular_acceleration_degrees_per_second_squared( "angular acceleration", "degrees/(second*second)", "deg/(s*s)", 1.0, 0.0, 1 );
114 
115 cUnitConverter const cSDH::uc_angular_acceleration_radians_per_second_squared( "angular acceleration", "radians/(second*second)", "rad/(s*s)", (2.0*M_PI)/360.0, 0.0, 3 );
116 
117 cUnitConverter const cSDH::uc_motor_current_ampere( "motor current", "Ampere", "A", 1.0, 0.0, 3 );
118 
119 cUnitConverter const cSDH::uc_motor_current_milliampere( "motor current", "milli Ampere", "mA", 1000.0, 0.0, 0 );
120 
121 cUnitConverter const cSDH::uc_position_millimeter( "position", "millimeter", "mm", 1.0, 0.0, 1 );
122 
123 cUnitConverter const cSDH::uc_position_meter( "position", "meter", "m", 1.0/1000.0, 0.0, 4 );
124 
125 
126 bool cSDH::IsVirtualAxis( int iAxis )
127 {
128  CheckIndex( iAxis, nb_all_axes, "axis" );
129  return (iAxis >= NUMBER_OF_AXES);
130 }
131 //----------------------------------------------------------------------
132 
133 
134 std::vector<double> cSDH::SetAxisValueVector( std::vector<int> const& axes,
135  std::vector<double> const& values,
136  pSetFunction ll_set,
137  pGetFunction ll_get,
138  cUnitConverter const* uc,
139  std::vector<double> const& min_values,
140  std::vector<double> const& max_values,
141  char const* name )
142 {
143  //---------------------
144  // Check parameters:
145 
146  if (axes.size() != values.size())
147  {
148  throw new cSDHErrorInvalidParameter( cMsg( "Lengths of axis indices and %s values vectors do not match (%ld != %ld)", name, axes.size(), values.size()) );
149  }
150 
151  int used = 0; // bit vector of used indices
152  std::vector<int>::const_iterator ai = axes.begin();
153  std::vector<double>::const_iterator vi = values.begin();
154  for ( axes.begin(), vi = values.begin();
155  ai != axes.end();
156  ai++, vi++ )
157  {
158  CheckIndex( *ai, nb_all_axes, name );
159  CheckRange( uc->ToInternal(*vi), min_values[ *ai ], max_values[ *ai ], name );
160 
161  used |= (1 << (*ai));
162  }
163  //
164  //---------------------
165  cdbg << "SetAxisValueVector: axes and values ok, used=" << used << "\n";
166 
167  //---------------------
168  // Prepare data to send:
169  // - ignore virtual axes
170  // - get and use values from SDH if given value is NaN
171  // - convert given
172 
173  cSimpleVector c_value; // space to store value to set from SDH for those axes where the given value is NaN
174  bool c_value_valid = false;
175 
176  cSimpleVector all_values; // space to store ordered full vector if all axes are to be set
177  cSimpleVector returned_values; // space to store ordered full vector with responses
178 
179 
180  // are all axes used?
181  if ( (used & all_axes_used) != all_axes_used )
182  {
183  // No, so we have to get current target values first
184  c_value = (comm_interface.*ll_get)( All, NULL );
185  c_value_valid = true;
186  }
187 
188  for ( ai = axes.begin(), vi = values.begin();
189  ai != axes.end();
190  ai++, vi++ )
191  {
192  double v;
193 
194  // Is it a virtual axis?
195  if ( IsVirtualAxis( *ai ) )
196  // yes, ignore virutal axes (low level functions know nothing about virtual axes)
197  continue ;
198 
199  // It is a normal axis.
200 
201  // Is a real value given?
202  if (SDH_ISNAN((*vi)))
203  {
204  // No, so use the value from the SDH.
205 
206  // Has that vector already been read from SDH?
207  if (!c_value_valid)
208  {
209  // No, so read it from SDH (this is done only once in this loop).
210  c_value = (comm_interface.*ll_get)( All, NULL );
211  c_value_valid = true;
212  }
213  v = c_value[ *ai ];
214  }
215  else
216  // only user given values must be converted to internal unit system
217  v = uc->ToInternal( *vi );
218 
219  // Now v is the axis value to set for axis *ai in internal units
220 
221  cdbg << "SetAxisValueVector: setting v=" << v << "\n";
222 
223  all_values[ *ai ] = v;
224  }
225 
226  returned_values = (comm_interface.*ll_set)( All, &(all_values[0]) );
227 
228  // convert returned_values to a std::vector
229  std::vector<double> rv( axes.size(), 0.0 ); // the vector to return
230 
231  int i;
232  for ( i=0, ai = axes.begin();
233  ai != axes.end();
234  i++, ai++ )
235  {
236  double v;
237 
238  if ( IsVirtualAxis( *ai ) )
239  v = 0.0;
240  else
241  v = returned_values[ *ai ];
242 
243  rv[i] = uc->ToExternal( v );
244  }
245 
246  return rv;
247 }
248 //----------------------------------------------------------------------
249 
250 
251 std::vector<double> cSDH::GetAxisValueVector( std::vector<int> const& axes,
252  pGetFunction ll_get,
253  cUnitConverter const* uc,
254  char const* name )
255 {
256 
257  //---------------------
258  // Check parameters:
259 
260  std::vector<int>::const_iterator ai;
261 
262  for ( ai = axes.begin();
263  ai != axes.end();
264  ai++ )
265  {
266  CheckIndex( *ai, nb_all_axes, name );
267  }
268  //
269  //---------------------
270 
271 
272  // We want to read (most likely) more than one value, so
273  // read all values at once in order to communicate as few as possible.
274  cSimpleVector all_values = (comm_interface.*ll_get)( All, NULL );
275 
276  std::vector<double> rv( axes.size(), 0.0 ); // the vector to return
277 
278  int i;
279  for ( i=0, ai = axes.begin();
280  ai != axes.end();
281  i++, ai++ )
282  {
283  double v;
284 
285  if ( IsVirtualAxis( *ai ) )
286  v = 0.0;
287  else
288  v = all_values[ *ai ];
289 
290  rv[i] = uc->ToExternal( v );
291  }
292 
293  return rv;
294 }
295 //----------------------------------------------------------------------
296 
297 
298 cSDH::cSDH( bool _use_radians, bool _use_fahrenheit, int _debug_level )
299  :
300  // call base class constructors:
301  cSDHBase( _debug_level ),
302 
303  // init member objects:
304  com( NULL ),
305  comm_interface( _debug_level-1 ),
307 {
308  cdbg.SetColor( "blue" );
309  cdbg.PDM( "Debug messages of cSDH are printed like this.\n" );
310 
311  //---------------------
312  // Initialize unit converters:
313  if ( _use_radians )
314  UseRadians();
315  else
316  UseDegrees();
317 
320 
321  if ( _use_fahrenheit )
322  {
324  }
325  else
326  {
329  }
330 
333 
336 
337  //---------------------
338 
339  //---------------------
340  // Initialize misc member variables
341 
344 
347 
349  assert( NUMBER_OF_FINGERS == 3 );
351  finger_number_of_axes[ 1 ] = 2;
352 
353 
355  std::vector<int>* f0_axis_index = new std::vector<int>( NUMBER_OF_AXES_PER_FINGER, 0 );
356  std::vector<int>* f1_axis_index = new std::vector<int>( NUMBER_OF_AXES_PER_FINGER, 0 );
357  std::vector<int>* f2_axis_index = new std::vector<int>( NUMBER_OF_AXES_PER_FINGER, 0 );
358  (*f0_axis_index)[0] = 0;
359  (*f0_axis_index)[1] = 1;
360  (*f0_axis_index)[2] = 2;
361 
362  (*f1_axis_index)[0] = 7;
363  (*f1_axis_index)[1] = 3;
364  (*f1_axis_index)[2] = 4;
365 
366  (*f2_axis_index)[0] = 0;
367  (*f2_axis_index)[1] = 5;
368  (*f2_axis_index)[2] = 6;
369 
370  finger_axis_index.assign( NUMBER_OF_FINGERS, *f0_axis_index );
371  finger_axis_index[1] = *f1_axis_index;
372  finger_axis_index[2] = *f2_axis_index;
373 
374 #if 0
375  f_eps_v = Numeric.array( [ eps for i in range( 0, NUMBER_OF_AXES_PER_FINGER ) ] );
377 
379  f_zeros_v = Numeric.array( [ 0.0 for i in range( 0, NUMBER_OF_AXES_PER_FINGER ) ] );
380 
382  f_ones_v = Numeric.array( [ 1.0 for i in range( 0, NUMBER_OF_AXES_PER_FINGER ) ] );
383 #endif
384 
386 
387  zeros_v.assign( nb_all_axes, 0.0 );
388  ones_v.assign( nb_all_axes, 1.0 );
389 
390  int i;
391  all_axes.assign( nb_all_axes, 0 );
392  for ( i = 0; i < nb_all_axes; i++ )
393  all_axes[i] = i;
394 
395  all_real_axes.assign( nb_all_axes-NUMBER_OF_VIRTUAL_AXES, 0 );
396  for ( i = 0; i < nb_all_axes-NUMBER_OF_VIRTUAL_AXES; i++ )
397  all_real_axes[i] = i;
398 
399  all_fingers.assign( NUMBER_OF_FINGERS, 0 );
400  for ( i = 0; i < NUMBER_OF_FINGERS; i++ )
401  all_fingers[i] = i;
402 
404  for ( i = 0; i < NUMBER_OF_TEMPERATURE_SENSORS; i++ )
406 
407  f_min_motor_current_v.assign( nb_all_axes, 0.0 );
408 
409  f_max_motor_current_v.assign( nb_all_axes, 1.0 );
410 
411 
412  f_min_angle_v.assign( nb_all_axes, -90.0 );
413  f_min_angle_v[ 0 ] = 0.0; // correct axis 0
414 
415  f_max_angle_v.assign( nb_all_axes, 90.0 );
416 
417 
418  f_min_velocity_v.assign( nb_all_axes, 0.0 );
419 
420  // use some default values for the max velocities. read the correct values later from firmware when we are connected
421  f_max_velocity_v.assign( nb_all_axes, 100.0 );
422  f_max_velocity_v[ 0 ] = 80.0; // correct axis 0
423 
424  // use some default values for the max accelerations. read the correct values later from firmware when we are connected
425  f_min_acceleration_v.assign( nb_all_axes, 0.0 );
426 
427  f_max_acceleration_v.push_back( 5000.0 );
428  f_max_acceleration_v.push_back( 400.0 );
429  f_max_acceleration_v.push_back( 1500.0 );
430  f_max_acceleration_v.push_back( 400.0 );
431  f_max_acceleration_v.push_back( 1500.0 );
432  f_max_acceleration_v.push_back( 400.0 );
433  f_max_acceleration_v.push_back( 1500.0 );
434  f_max_acceleration_v.push_back( 5000.0 ); // virtual axis (setting to 0.0 would make SetAxisTargetAcceleration( All, x ) invalid for all x != 0.0
435 
437  grip_max_velocity = 100.0;
438 
439  //---------------------
440  // set kinematic variables
441 
442  l1 = 86.5;
443 
444  l2 = 68.5;
445 
446  d = 66;
447 
448  h = 17.0;
449 
450  std::vector<double>* f0_offset = new std::vector<double>( 3, 0.0 );
451  std::vector<double>* f1_offset = new std::vector<double>( 3, 0.0 );
452  std::vector<double>* f2_offset = new std::vector<double>( 3, 0.0 );
453 
454  (*f0_offset)[ 0 ] = d/2.0; // x
455  (*f0_offset)[ 1 ] = d/2.0*tan( DegToRad(30.0) ); // y
456  (*f0_offset)[ 2 ] = h; // z
457 
458  (*f1_offset)[ 0 ] = 0.0; // x
459  (*f1_offset)[ 1 ] = -d/(2.0*cos( DegToRad(30.0) ));// y
460  (*f1_offset)[ 2 ] = h; // z
461 
462  (*f2_offset)[ 0 ] = -d/2.0; // x
463  (*f2_offset)[ 1 ] = d/2.0*tan( DegToRad(30.0) ); // y
464  (*f2_offset)[ 2 ] = h; // z
465 
466  offset.assign( NUMBER_OF_FINGERS, *f0_offset );
467  offset[1] = *f1_offset;
468  offset[2] = *f2_offset;
469  //
470  //---------------------
471 }
472 //----------------------------------------------------------------------
473 
474 
476 {
477  if ( IsOpen() )
478  {
479  cdbg << "Cleanup: Closing port in destructor ~cSDH\n";
480  Close();
481  }
482 
483  if ( com )
484  {
485  delete com;
486  com = NULL;
487  }
488 }
489 //----------------------------------------------------------------------
490 
491 
492 std::vector<int> cSDH::ToIndexVector( int index, std::vector<int>& all_replacement, int maxindex, char const* name )
493 {
494  if (index == All)
495  {
496  return all_replacement;
497  }
498  else
499  {
500  CheckIndex( index, maxindex, name );
501  return std::vector<int>( 1, index );
502  }
503 }
504 //-----------------------------------------------------------------
505 
506 #if 0
507 std::vector<double> cSDH::ToValueVector( double value, int length, pDoubleUnitConverterFunction convert )
508 {
509  return std::vector<double>( length, *convert( value ) );
510 }
511 #endif
512 
513 //-----------------------------------------------------------------
515 {
516  switch (mode)
517  {
518  case eMCM_MOVE: return &cSDHSerial::ilim;
519  case eMCM_GRIP: return &cSDHSerial::igrip;
520  case eMCM_HOLD: return &cSDHSerial::ihold;
521  default:
522  throw new cSDHErrorInvalidParameter( cMsg( "Unknown mode '%d', not in [0..%d]!", int(mode), eMCM_DIMENSION-1) );
523  }
524 }
525 //-----------------------------------------------------------------
526 
527 
528 #if 0
529 std::vector<std::vector<double> > cSDH::_GetHandXYZ( double angles )
530 {
531  f0_angles = [ angles[i] for i in finger_axis_index[0] ];
532  f1_angles = [ angles[i] for i in finger_axis_index[1] ];
533  f2_angles = [ angles[i] for i in finger_axis_index[2] ];
534 
535  return [_GetFingerXYZ( 0, f0_angles ), // finger 0
536  _GetFingerXYZ( 1, f1_angles ), // finger 1
537  _GetFingerXYZ( 2, f2_angles )]; // finger 2
538 }
539 //----------------------------------------------------------------------
540 #endif
541 
542 
543 //-----------------------------------------------------------------
544 std::vector<double> cSDH::_GetFingerXYZ( int fi, std::vector<double> r_angles )
545 {
546  std::vector<double> rv(3,0.0);
547 
548  double fac_x, fac_y;
549  switch (fi)
550  {
551  case 0:
552  fac_x = -1.0;
553  fac_y = -1.0;
554  break;
555  case 1:
556  fac_x = 1.0;
557  fac_y = 1.0;
558  break;
559  case 2:
560  fac_x = 1.0;
561  fac_y = -1.0;
562  break;
563  default:
564  throw new cSDHErrorInvalidParameter( cMsg( "Unexpected finger index '%d' not in [0..3]!", fi ) );
565  }
566 
567  double s_b = sin( r_angles[1] );
568  double s_bc = sin( r_angles[1] + r_angles[2] );
569  double l1_s_b_l2_s_bc = (l1*s_b + l2*s_bc);
570 
571  rv[0] = fac_x * (l1_s_b_l2_s_bc) * sin( r_angles[0] ) + offset[ fi ][0]; // x
572  rv[1] = fac_y * (l1_s_b_l2_s_bc) * cos( r_angles[0] ) + offset[ fi ][1]; // y
573  rv[2] = l1*cos( r_angles[1] ) + l2*cos( r_angles[1] + r_angles[2] ) + offset[ fi][2]; // z
574 
575  return rv;
576 }
577 //----------------------------------------------------------------------
578 
579 
580 void cSDH::UseRadians( void )
581 {
582  // set unit convert for (axis) angles:
584 
585  // set unit convert for (axis) angular velocities:
587 
588  // set unit convert for (axis) angular accelerations:
590 }
591 //-----------------------------------------------------------------
592 
593 
594 void cSDH::UseDegrees( void )
595 {
596  // set unit convert for (axis) angles:
598 
599  // set unit convert for (axis) angular velocities:
601 
602  // set unit convert for (axis) angular accelerations:
604 }
605 //-----------------------------------------------------------------
606 
607 
608 int cSDH::GetFingerNumberOfAxes( int iFinger )
609 {
610  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
611  return finger_number_of_axes[iFinger];
612 }
613 //-----------------------------------------------------------------
614 
615 
616 int cSDH::GetFingerAxisIndex( int iFinger, int iFingerAxis )
617 {
618  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
619  CheckIndex( iFingerAxis, NUMBER_OF_AXES_PER_FINGER, "finger axis" );
620 
621  return finger_axis_index[ iFinger ][ iFingerAxis ];
622 }
623 //-----------------------------------------------------------------
624 
625 
626 char const* cSDH::GetLibraryRelease( void )
627 {
628  return PROJECT_RELEASE;
629 }
630 //----------------------------------------------------------------------
631 
632 
633 char const* cSDH::GetLibraryName( void )
634 {
635  return PROJECT_NAME;
636 }
637 //----------------------------------------------------------------------
638 
639 
640 char const* cSDH::GetFirmwareRelease( void )
641 {
642  if ( ! comm_interface.IsOpen() )
643  throw new cSDHErrorCommunication( "No connection to SDH" );
644 
645  return comm_interface.ver();
646 }
647 //----------------------------------------------------------------------
648 
650 {
652 }
653 //----------------------------------------------------------------------
654 
656 {
657  return ! strcmp( GetFirmwareRelease(), GetFirmwareReleaseRecommended() );
658 }
659 //----------------------------------------------------------------------
660 
661 char const* cSDH::GetInfo( char const* what )
662 {
663  cdbg << "GetInfo: " << what << " is requested\n";
664 
665  if ( ! strcmp( what,"release" ) || ! strcmp( what, "release-library" ) )
666  return PROJECT_RELEASE;
667  if ( ! strcmp( what, "date" ) || ! strcmp( what, "date-library" ) )
668  return PROJECT_DATE;
669  if ( ! strcmp( what, "release-firmware-recommended" ) )
671 
672  if ( ! comm_interface.IsOpen() )
673  throw new cSDHErrorCommunication("Interface to SDH is not open");
674 
675  if ( ! strcmp( what, "release-firmware" ) )
676  return comm_interface.ver();
677  if ( ! strcmp( what, "date-firmware") )
678  return comm_interface.ver_date();
679  if ( ! strcmp( what, "release-soc" ) )
680  return comm_interface.soc();
681  if ( ! strcmp( what, "date-soc" ) )
682  return comm_interface.soc_date();
683  if ( ! strcmp( what, "date-soc" ) )
684  return comm_interface.soc_date();
685  if ( ! strcmp( what, "id-sdh") )
686  return comm_interface.id();
687  if ( ! strcmp( what, "sn-sdh" ) )
688  return comm_interface.sn();
689  return "?";
690 }
691 //----------------------------------------------------------------------
692 
693 
694 std::vector<double> cSDH::GetTemperature( std::vector<int> const& sensors )
695 {
696  cSimpleVector temperatures_axes = comm_interface.temp();
697  cSimpleVector temperatures_electronics = comm_interface.temp_electronics();
698 
699  std::vector<double> rv;
700 
701  for ( std::vector<int>::const_iterator si = sensors.begin();
702  si != sensors.end();
703  si++ )
704  {
705  CheckIndex( *si, NUMBER_OF_TEMPERATURE_SENSORS, "temperature sensor" );
706 
707  if ( *si < NUMBER_OF_AXES )
708  rv.push_back( uc_temperature->ToExternal( temperatures_axes[ *si ] ) );
709  else
710  rv.push_back( uc_temperature->ToExternal( temperatures_electronics[ *si - NUMBER_OF_AXES ] ) );
711  }
712  return rv;
713 }
714 //----------------------------------------------------------------------
715 
716 
717 double cSDH::GetTemperature( int iSensor )
718 {
719  CheckIndex( iSensor, NUMBER_OF_TEMPERATURE_SENSORS, "temperature sensor" );
720 
721  cSimpleVector temperatures;
722  if ( iSensor < NUMBER_OF_AXES )
723  {
724  temperatures = comm_interface.temp();
725  return uc_temperature->ToExternal( temperatures[ iSensor ] );
726  }
727  temperatures = comm_interface.temp_electronics();
728  return uc_temperature->ToExternal( temperatures[ iSensor-NUMBER_OF_AXES ] );
729 }
730 //----------------------------------------------------------------------
731 
732 
733 void cSDH::OpenRS232( int _port, unsigned long _baudrate, double _timeout, char const* _device_format_string )
734 {
735  //---------------------
736  // Try to create a cSDHSerial object and save as member comm_interface:
737 
738  if ( com )
739  {
740  delete com;
741  com = NULL;
742  }
743  com = new cRS232( _port, _baudrate, _timeout, _device_format_string );
744  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-2 (cSDHSerial uses level-1 already)
746 
747  // Now that we're connected update settings from the connected SDH
749 
750  cdbg << "cSDH.OpenRS232() successfully opened RS232 port.\n";
751 }
752 //----------------------------------------------------------------------
753 
754 void cSDH::OpenCAN_ESD( int _net, unsigned long _baudrate, double _timeout, Int32 _id_read, Int32 _id_write )
755 {
756 #if WITH_ESD_CAN
757  //---------------------
758  // Try to create a cSDHSerial object and save as member comm_interface:
759 
760  if ( _timeout < 0.0 )
761  _timeout = 0.0;
762 
763  if ( com )
764  {
765  delete com;
766  com = NULL;
767  }
768  com = new cCANSerial_ESD( _net, _baudrate, _timeout, _id_read, _id_write );
769  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-1
771 
772  // Now that we're connected update settings from the connected SDH
774 
775  cdbg << "cSDH.OpenCAN_ESD() successfully opened CAN port.\n";
776 #else
777  throw new cSDHErrorInvalidParameter( cMsg( "Cannot open ESD CAN net: The SDHLibrary was compiled without ESD CAN support" ) );
778 #endif
779 }
780 //----------------------------------------------------------------------
781 
782 void cSDH::OpenCAN_ESD( tDeviceHandle _ntcan_handle, double _timeout, Int32 _id_read, Int32 _id_write )
783 {
784 #if WITH_ESD_CAN
785  //---------------------
786  // Try to create a cSDHSerial object and save as member comm_interface:
787 
788  if ( _timeout < 0.0 )
789  _timeout = 0.0;
790 
791  if ( com )
792  {
793  delete com;
794  com = NULL;
795  }
796  com = new cCANSerial_ESD( _ntcan_handle, _timeout, _id_read, _id_write );
797  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-1
799 
800  // Now that we're connected update settings from the connected SDH
802 
803  cdbg << "cSDH.OpenCAN_ESD() successfully opened CAN port.\n";
804 #else
805  throw new cSDHErrorInvalidParameter( cMsg( "Cannot open ESD CAN handle: The SDHLibrary was compiled without ESD CAN support" ) );
806 #endif
807 }
808 //----------------------------------------------------------------------
809 
810 void cSDH::OpenCAN_PEAK( unsigned long _baudrate, double _timeout, Int32 _id_read, Int32 _id_write, char const* _device )
811 {
812 #if WITH_PEAK_CAN
813  //---------------------
814  // Try to create a cSDHSerial object and save as member comm_interface:
815 
816  if ( _timeout < 0.0 )
817  _timeout = 0.0;
818 
819  if ( com )
820  {
821  delete com;
822  com = NULL;
823  }
824  com = new cCANSerial_PEAK( _baudrate, _timeout, _id_read, _id_write, _device );
825  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-1
827 
828  // Now that we're connected update settings from the connected SDH
830 
831  cdbg << "cSDH.OpenCAN_PEAK() successfully opened PEAK CAN device \"" << _device << "\".\n";
832 #else
833  throw new cSDHErrorInvalidParameter( cMsg( "Cannot open PEAK CAN device: The SDHLibrary was compiled without PEAK CAN support" ) );
834 #endif
835 }
836 //----------------------------------------------------------------------
837 
838 void cSDH::OpenCAN_PEAK( tDeviceHandle _handle, double _timeout, Int32 _id_read, Int32 _id_write )
839 {
840 #if WITH_PEAK_CAN
841  //---------------------
842  // Try to create a cSDHSerial object and save as member comm_interface:
843 
844  if ( _timeout < 0.0 )
845  _timeout = 0.0;
846 
847  if ( com )
848  {
849  delete com;
850  com = NULL;
851  }
852  com = new cCANSerial_PEAK( _handle, _timeout, _id_read, _id_write );
853  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-1
855 
856  // Now that we're connected update settings from the connected SDH
858 
859  cdbg << "cSDH.OpenCAN_PEAK() successfully reopened CAN device.\n";
860 #else
861  throw new cSDHErrorInvalidParameter( cMsg( "Cannot open PEAK CAN handle: The SDHLibrary was compiled without PEAK CAN support" ) );
862 #endif
863 }
864 //----------------------------------------------------------------------
865 
866 void cSDH::OpenTCP( char const* _tcp_adr, int _tcp_port, double _timeout )
867 {
868  //---------------------
869  // Try to create a cTCPSerial object and save as member comm_interface:
870 
871  if ( com )
872  {
873  delete com;
874  com = NULL;
875  }
876  com = new cTCPSerial( _tcp_adr, _tcp_port, _timeout );
877  com->dbg.SetFlag( debug_level > 2 ); // set debug level of low level com interface to our own level-1
879 
880  // Now that we're connected update settings from the connected SDH
882 
883  cdbg << "cSDH.OpenTCP() successfully opened TCP connection to \"" << _tcp_adr << ":" << _tcp_port << "\"\n";
884 }
885 //----------------------------------------------------------------------
886 
887 
888 void cSDH::Close( bool leave_enabled )
889 {
890  if (comm_interface.IsOpen())
891  {
892  try
893  {
894  if (! leave_enabled )
895  {
896  cdbg << "Switching off power before closing connection to SDH\n";
897  comm_interface.power( All, &(zeros_v[0]) );
898  }
899 
901  cdbg << "Connection to SDH closed.\n";
902  }
903  catch ( cSDHLibraryException* e )
904  {
905  cdbg << "Ignoring exception while closing connection to SDH (" << e->what() << ")\n";
906  delete e;
907  }
908  }
909  else
910  {
911  throw new cSDHErrorCommunication( "No connection to SDH" );
912  }
913 }
914 //-----------------------------------------------------------------
915 
916 
917 bool cSDH::IsOpen( void )
918  throw()
919 {
920  return comm_interface.IsOpen();
921 }
922 //-----------------------------------------------------------------
923 
924 
926 {
927  // switch off controllers
928  comm_interface.power( All, &(zeros_v[0]) );
929 
930  // save current actual axis angles as new target axis angles
931  cSimpleVector angles = comm_interface.pos( All );
932  // limit angles to allowed ranges:
934  comm_interface.p( All, &(angles[0]) );
935 }
936 //-----------------------------------------------------------------
937 
938 
939 void cSDH::Stop( void )
940 {
942 }
943 //-----------------------------------------------------------------
944 
945 
947 {
948  if (controller >= eCT_DIMENSION)
949  {
950  throw new cSDHErrorInvalidParameter( cMsg( "Invalid controller type %d = '%s'", int(controller), GetStringFromControllerType(controller) ) );
951  }
952 
953  if ( controller > eCT_POSE && CompareReleases( release_firmware.c_str(), "0.0.2.6" ) < 0 )
954  {
955  throw new cSDHErrorInvalidParameter( cMsg( "Controller type %d not available in firmware %s of currently attached SDH", controller, release_firmware.c_str() ) );
956  }
957 
958 
959  if ( controller == eCT_POSE && CompareReleases( release_firmware.c_str(), "0.0.2.6" ) < 0 )
960  // nothing more to do here: for firmwares prior to 0.0.2.6 the controller is fixed to eCT_POSE anyway
961  // (and setting the controller would yield an error from the firmware (unknown command))
962  controller_type = controller;
963  else
964  controller_type = comm_interface.con(controller);
965 
967 }
968 //-----------------------------------------------------------------
969 
970 
972 {
973  if ( CompareReleases( release_firmware.c_str(), "0.0.2.6" ) < 0.0 )
974  {
975  // cannot read controller in firmwares prior to 0.0.2.6 where controller is fixed to POSE
977  }
978  else
980 
981  return controller_type;
982 }
983 //-----------------------------------------------------------------
984 
985 
987 {
988  comm_interface.vp( velocity_profile );
989 }
990 //-----------------------------------------------------------------
991 
992 
994 {
996  return evp;
997  //return (eVelocityProfile) comm_interface.vp();
998 }
999 //----------------------------------------------------------------------
1000 
1001 
1002 void cSDH::SetAxisMotorCurrent( std::vector<int> const& axes, std::vector<double> const& motor_currents, eMotorCurrentMode mode )
1003 {
1004  SetAxisValueVector( axes, motor_currents,
1009  "motor current" );
1010 }
1011 //----------------------------------------------------------------------
1012 
1013 
1014 void cSDH::SetAxisMotorCurrent( int iAxis, double motor_current, eMotorCurrentMode mode )
1015 {
1016  std::vector<int> axes = ToIndexVector( iAxis, all_axes, nb_all_axes, "axis" );
1017  // now axes is a vector of all axis indices to access
1018 
1019  std::vector<double> motor_currents( axes.size(), motor_current );
1020  // now motor_currents is a vector of all axis motor currents in external unit
1021  // system
1022 
1023  SetAxisValueVector( axes, motor_currents,
1028  "motor current" );
1029 }
1030 //----------------------------------------------------------------------
1031 
1032 
1033 
1034 std::vector<double> cSDH::GetAxisMotorCurrent( std::vector<int> const& axes, eMotorCurrentMode mode )
1035 {
1036  return GetAxisValueVector( axes,
1039  "motor current" );
1040 }
1041 //----------------------------------------------------------------------
1042 
1043 
1045 {
1046  CheckIndex( iAxis, nb_all_axes, "axis" );
1047 
1048  pGetFunction get_func = GetMotorCurrentModeFunction( mode );
1049 
1050  return uc_motor_current->ToExternal( (comm_interface.*get_func)( iAxis, NULL )[ iAxis ] );
1051 }
1052 //----------------------------------------------------------------------
1053 
1054 
1055 void cSDH::SetAxisEnable( std::vector<int> const& axes, std::vector<double> const& states )
1056 {
1057  SetAxisValueVector( axes, states,
1060  &uc_identity,
1061  zeros_v, ones_v,
1062  "enable/disable" );
1063 }
1064 //----------------------------------------------------------------------
1065 
1066 
1067 void cSDH::SetAxisEnable( int iAxis, double state )
1068 {
1069  std::vector<int> axes = ToIndexVector( iAxis, all_axes, nb_all_axes, "axis" );
1070  // now axes is a list of all axis indices to access
1071 
1072  std::vector<double> states( axes.size(), state );
1073  // now states is a list of all axis states
1074 
1075  SetAxisValueVector( axes, states,
1078  &uc_identity,
1079  zeros_v, ones_v,
1080  "enable/disable" );
1081 }
1082 //----------------------------------------------------------------------
1083 
1084 
1085 void cSDH::SetAxisEnable( std::vector<int> const& axes, std::vector<bool> const& states )
1086 {
1087  std::vector<double> dstates( states.size(), 0.0 );
1088  std::vector<bool>::const_iterator bi;
1089  std::vector<double>::iterator di;
1090  for ( bi = states.begin(), di = dstates.begin();
1091  bi != states.end() && di != dstates.end();
1092  bi++, di++ )
1093  *di = *bi;
1094 
1095  SetAxisEnable( axes, dstates );
1096 }
1097 //----------------------------------------------------------------------
1098 
1099 
1100 void cSDH::SetAxisEnable( int iAxis, bool state )
1101 {
1102  SetAxisEnable( iAxis, state ? 1.0 : 0.0 );
1103 }
1104 //----------------------------------------------------------------------
1105 
1106 
1107 
1108 std::vector<double> cSDH::GetAxisEnable( std::vector<int> const& axes )
1109 {
1110  return GetAxisValueVector( axes,
1112  &uc_identity,
1113  "enabled/disabled" );
1114 }
1115 //-----------------------------------------------------------------
1116 
1117 
1118 double cSDH::GetAxisEnable( int iAxis )
1119 {
1120  CheckIndex( iAxis, nb_all_axes, "axis" );
1121 
1122  return comm_interface.power( iAxis, NULL )[ iAxis ];
1123 }
1124 //-----------------------------------------------------------------
1125 
1126 
1127 std::vector<cSDH::eAxisState> cSDH::GetAxisActualState( std::vector<int> const& axes )
1128 {
1129  std::vector<double> fstates = GetAxisValueVector( axes,
1131  &uc_identity,
1132  "state" );
1133  std::vector<eAxisState> estates;
1134  std::vector<double>::const_iterator si;
1135 
1136  for ( si = fstates.begin();
1137  si != fstates.end();
1138  si++ )
1139  {
1140  estates.push_back( eAxisState( int( *si ) ) );
1141  }
1142 
1143  return estates;
1144 }
1145 //----------------------------------------------------------------------
1146 
1147 
1149 {
1150  CheckIndex( iAxis, nb_all_axes, "axis" );
1151 
1152 
1153  return eAxisState( int( comm_interface.state( iAxis, NULL )[ iAxis ] ) );
1154 }
1155 //----------------------------------------------------------------------
1156 
1157 
1158 void cSDH::WaitAxis( std::vector<int> const& axes, double timeout )
1159 {
1160  cSimpleTime start_time;
1161  bool finished;
1162 
1163  eAxisState busy;
1164  if ( controller_type == eCT_POSE )
1165  busy = eAS_POSITIONING;
1166  else
1167  busy = eAS_SPEED_MODE;
1168 
1169  do
1170  {
1171  std::vector<eAxisState> states = GetAxisActualState( axes );
1172 
1173  finished = true;
1174 
1175  std::vector<eAxisState>::const_iterator si;
1176  for( si = states.begin();
1177  si != states.end();
1178  si++ )
1179  {
1180  finished = finished && (*si != busy );
1181  }
1182 
1183  if (!finished && timeout >= 0.0 && start_time.Elapsed() > timeout )
1184  {
1185  throw new cSDHErrorCommunication( cMsg( "Timeout in WaitAxis" ) );
1186  }
1187  } while ( ! finished );
1188 }
1189 //-----------------------------------------------------------------
1190 
1191 
1192 void cSDH::WaitAxis( int iAxis, double timeout )
1193 {
1194  std::vector<int> axes;
1195 
1196  if (iAxis == All)
1197  axes = all_axes;
1198  else
1199  {
1200  CheckIndex( iAxis, nb_all_axes, "axis" );
1201  axes.push_back( iAxis );
1202  }
1203 
1204  WaitAxis( axes, timeout );
1205 }
1206 //-----------------------------------------------------------------
1207 
1208 
1209 void cSDH::SetAxisTargetAngle( std::vector<int> const& axes, std::vector<double> const& angles )
1210 {
1211  SetAxisValueVector( axes, angles,
1212  &cSDHSerial::p,
1213  &cSDHSerial::p,
1214  uc_angle,
1216  "target angle" );
1217 }
1218 //----------------------------------------------------------------------
1219 
1220 
1221 void cSDH::SetAxisTargetAngle( int iAxis, double angle )
1222 {
1223  std::vector<int> axes = ToIndexVector( iAxis, all_axes, nb_all_axes, "axis" );
1224  // now axes is a vector of all axis indices to access
1225 
1226  std::vector<double> angles( axes.size(), angle );
1227  // now angles is a vector of all target axis angles in external unit system
1228 
1229  SetAxisValueVector( axes, angles,
1230  &cSDHSerial::p,
1231  &cSDHSerial::p,
1232  uc_angle,
1234  "target angle" );
1235 }
1236 //----------------------------------------------------------------------
1237 
1238 
1239 std::vector<double> cSDH::SetAxisTargetGetAxisActualAngle( std::vector<int> const& axes, std::vector<double> const& angles )
1240 {
1241  return SetAxisValueVector( axes, angles,
1243  &cSDHSerial::p, // when only some target angles are set then we have to get the others from the SDH first with the ordinary cSDHSerial::p()
1244  uc_angle,
1246  "set target, get actual angle" );
1247 }
1248 //----------------------------------------------------------------------
1249 
1250 
1251 std::vector<double> cSDH::GetAxisTargetAngle( std::vector<int> const& axes )
1252 {
1253  return GetAxisValueVector( axes,
1254  &cSDHSerial::p,
1255  uc_angle,
1256  "target angle" );
1257 }
1258 //----------------------------------------------------------------------
1259 
1260 
1261 double cSDH::GetAxisTargetAngle( int iAxis )
1262 {
1263  CheckIndex( iAxis, nb_all_axes, "axis" );
1264 
1265  return uc_angle->ToExternal( comm_interface.p( iAxis, NULL )[ iAxis ] );
1266 }
1267 //----------------------------------------------------------------------
1268 
1269 
1270 std::vector<double> cSDH::GetAxisActualAngle( std::vector<int> const& axes )
1271 {
1272  return GetAxisValueVector( axes,
1273  &cSDHSerial::pos,
1274  uc_angle,
1275  "target angle" );
1276 }
1277 //----------------------------------------------------------------------
1278 
1279 
1280 double cSDH::GetAxisActualAngle( int iAxis )
1281 {
1282  CheckIndex( iAxis, nb_all_axes, "axis" );
1283 
1284  return uc_angle->ToExternal( comm_interface.pos( iAxis )[ iAxis ] );
1285 }
1286 //----------------------------------------------------------------------
1287 
1288 
1289 void cSDH::SetAxisTargetVelocity( std::vector<int> const& axes, std::vector<double> const& velocities )
1290 {
1291  SetAxisValueVector( axes, velocities,
1292  &cSDHSerial::v,
1293  &cSDHSerial::v,
1296  "target velocity" );
1297 }
1298 //----------------------------------------------------------------------
1299 
1300 
1301 void cSDH::SetAxisTargetVelocity( int iAxis, double velocity )
1302 {
1303  std::vector<int> axes = ToIndexVector( iAxis, all_axes, nb_all_axes, "axis" );
1304  // now axes is a vector of all axis indices to access
1305 
1306  std::vector<double> velocities( axes.size(), velocity );
1307  // now velocities is a vector of all target axis velocities in external unit system
1308 
1309  SetAxisValueVector( axes, velocities,
1310  &cSDHSerial::v,
1311  &cSDHSerial::v,
1314  "target velocity" );
1315 }
1316 //----------------------------------------------------------------------
1317 
1318 std::vector<double> cSDH::SetAxisTargetGetAxisActualVelocity( std::vector<int> const& axes, std::vector<double> const& velocities )
1319 {
1320  return SetAxisValueVector( axes, velocities,
1322  &cSDHSerial::v, // when not all target velocities are set then we have to get the others from the SDH first with the ordinary cSDHSerial::v()
1325  "set target, get actual velocity" );
1326 }
1327 //----------------------------------------------------------------------
1328 
1329 std::vector<double> cSDH::GetAxisTargetVelocity( std::vector<int> const& axes )
1330 {
1331  return GetAxisValueVector( axes,
1332  &cSDHSerial::v,
1334  "target velocity" );
1335 }
1336 //----------------------------------------------------------------------
1337 
1338 
1339 double cSDH::GetAxisTargetVelocity( int iAxis )
1340 {
1341  CheckIndex( iAxis, nb_all_axes, "axis" );
1342 
1343  return uc_angular_velocity->ToExternal( comm_interface.v( iAxis, NULL )[ iAxis ] );
1344 }
1345 //----------------------------------------------------------------------
1346 
1347 
1348 std::vector<double> cSDH::GetAxisLimitVelocity( std::vector<int> const& axes )
1349 {
1350  if ( CompareReleases( release_firmware.c_str(), "0.0.2.1" ) < 0 )
1351  {
1352  // if firmware is older than "0.0.2.1" then use fixed default:
1353  double all_velocities[] = { 85.7, 200.0, 157.8, 200.0, 157.8, 200.0, 157.8, 85.7 };
1354  std::vector<double> rv;
1355  std::vector<int>::const_iterator ai;
1356 
1357  for ( ai = axes.begin();
1358  ai != axes.end();
1359  ai++ )
1360  {
1361  rv.push_back( uc_angular_velocity->ToExternal( all_velocities[ *ai ] ) );
1362  }
1363  return rv;
1364  }
1365 
1366  return GetAxisValueVector( axes,
1369  "velocity limit" );
1370 }
1371 //----------------------------------------------------------------------
1372 
1373 
1374 std::vector<double> cSDH::GetAxisLimitAcceleration( std::vector<int> const& axes )
1375 {
1376  if ( CompareReleases( release_firmware.c_str(), "0.0.2.7" ) < 0 )
1377  {
1378  // firmware before 0.0.2.7 does not provide the acceleration limit
1379  // so use fake default:
1380  double all_accelerations[] = { 5000.0, 400.0, 1500.0, 400.0, 1500.0, 400.0, 1500.0, 400.0 };
1381  std::vector<double> rv;
1382  std::vector<int>::const_iterator ai;
1383 
1384  for ( ai = axes.begin();
1385  ai != axes.end();
1386  ai++ )
1387  {
1388  rv.push_back( uc_angular_acceleration->ToExternal( all_accelerations[ *ai ] ) );
1389  }
1390  return rv;
1391  }
1392 
1393  return GetAxisValueVector( axes,
1396  "acceleration limit" );
1397 }
1398 //----------------------------------------------------------------------
1399 
1400 
1401 double cSDH::GetAxisLimitVelocity( int iAxis )
1402 {
1403  std::vector<int> axes( 1, iAxis );
1404 
1405  return GetAxisLimitVelocity( axes )[0];
1406 }
1407 //----------------------------------------------------------------------
1408 
1409 
1411 {
1412  std::vector<int> axes( 1, iAxis );
1413 
1414  return GetAxisLimitAcceleration( axes )[0];
1415 }
1416 //----------------------------------------------------------------------
1417 
1418 
1419 std::vector<double> cSDH::GetAxisActualVelocity( std::vector<int>const& axes )
1420 {
1421  return GetAxisValueVector( axes,
1422  &cSDHSerial::vel,
1424  "actual velocity" );
1425 }
1426 //----------------------------------------------------------------------
1427 
1428 
1429 double cSDH::GetAxisActualVelocity( int iAxis )
1430 {
1431  CheckIndex( iAxis, nb_all_axes, "axis" );
1432 
1433  return uc_angular_velocity->ToExternal( comm_interface.vel( iAxis )[ iAxis ] );
1434 }
1435 //----------------------------------------------------------------------
1436 
1437 
1438 std::vector<double> cSDH::GetAxisReferenceVelocity( std::vector<int>const& axes )
1439 {
1440  return GetAxisValueVector( axes,
1443  "reference velocity" );
1444 }
1445 //----------------------------------------------------------------------
1446 
1447 
1449 {
1450  CheckIndex( iAxis, nb_all_axes, "axis" );
1451 
1452  return uc_angular_velocity->ToExternal( comm_interface.rvel( iAxis )[ iAxis ] );
1453 }
1454 //----------------------------------------------------------------------
1455 
1456 
1457 void cSDH::SetAxisTargetAcceleration( std::vector<int>const& axes, std::vector<double>const& accelerations )
1458 {
1459  SetAxisValueVector( axes, accelerations,
1460  &cSDHSerial::a,
1461  &cSDHSerial::a,
1464  "target acceleration" );
1465 }
1466 //----------------------------------------------------------------------
1467 
1468 
1469 void cSDH::SetAxisTargetAcceleration( int iAxis, double acceleration )
1470 {
1471  std::vector<int> axes = ToIndexVector( iAxis, all_axes, nb_all_axes, "axis" );
1472  // now axes is a vector of all axis indices to access
1473 
1474  std::vector<double> accelerations( axes.size(), acceleration );
1475  // now accelerations is a vector of all target axis accelerations in external unit system
1476 
1477  SetAxisValueVector( axes, accelerations,
1478  &cSDHSerial::a,
1479  &cSDHSerial::a,
1482  "target acceleration" );
1483 }
1484 //----------------------------------------------------------------------
1485 
1486 
1487 std::vector<double> cSDH::GetAxisTargetAcceleration( std::vector<int>const& axes )
1488 {
1489  return GetAxisValueVector( axes,
1490  &cSDHSerial::a,
1492  "target acceleration" );
1493 }
1494 //----------------------------------------------------------------------
1495 
1496 
1498 {
1499  CheckIndex( iAxis, nb_all_axes, "axis" );
1500 
1501  return uc_angular_acceleration->ToExternal( comm_interface.a( iAxis, NULL )[ iAxis ] );
1502 }
1503 //----------------------------------------------------------------------
1504 
1505 
1506 std::vector<double> cSDH::GetAxisMinAngle( std::vector<int> const& axes )
1507 {
1508  std::vector<double> rv( axes.size(), 0.0 );
1509 
1510  std::vector<int>::const_iterator ai;
1511  std::vector<double>::iterator vi;
1512  for( ai = axes.begin(), vi = rv.begin();
1513  ai != axes.end();
1514  ai++, vi++ )
1515  {
1516  CheckIndex( *ai, nb_all_axes, "axis" );
1517  *vi = uc_angle->ToExternal( f_min_angle_v[ *ai ] );
1518  }
1519 
1520  return rv;
1521 }
1522 //----------------------------------------------------------------------
1523 
1524 
1525 double cSDH::GetAxisMinAngle( int iAxis )
1526 {
1527  CheckIndex( iAxis, nb_all_axes, "axis" );
1528 
1529  return uc_angle->ToExternal( f_min_angle_v[ iAxis ] );
1530 }
1531 //----------------------------------------------------------------------
1532 
1533 
1534 std::vector<double> cSDH::GetAxisMaxAngle( std::vector<int> const& axes )
1535 {
1536  std::vector<double> rv( axes.size(), 0.0 );
1537 
1538  std::vector<int>::const_iterator ai;
1539  std::vector<double>::iterator vi;
1540  for( ai = axes.begin(), vi = rv.begin();
1541  ai != axes.end();
1542  ai++, vi++ )
1543  {
1544  CheckIndex( *ai, nb_all_axes, "axis" );
1545  *vi = uc_angle->ToExternal( f_max_angle_v[ *ai ] );
1546  }
1547 
1548  return rv;
1549 }
1550 //----------------------------------------------------------------------
1551 
1552 
1553 double cSDH::GetAxisMaxAngle( int iAxis )
1554 {
1555  CheckIndex( iAxis, nb_all_axes, "axis" );
1556 
1557  return uc_angle->ToExternal( f_max_angle_v[ iAxis ] );
1558 }
1559 //----------------------------------------------------------------------
1560 
1561 
1562 std::vector<double> cSDH::GetAxisMaxVelocity( std::vector<int> const& axes )
1563 {
1564  std::vector<double> rv( axes.size(), 0.0 );
1565 
1566  std::vector<int>::const_iterator ai;
1567  std::vector<double>::iterator vi;
1568  for( ai = axes.begin(), vi = rv.begin();
1569  ai != axes.end();
1570  ai++, vi++ )
1571  {
1572  CheckIndex( *ai, nb_all_axes, "axis" );
1573  *vi = uc_angle->ToExternal( f_max_velocity_v[ *ai ] );
1574  }
1575 
1576  return rv;
1577 }
1578 //----------------------------------------------------------------------
1579 
1580 
1581 double cSDH::GetAxisMaxVelocity( int iAxis )
1582 {
1583  CheckIndex( iAxis, nb_all_axes, "axis" );
1584 
1585  return uc_angle->ToExternal( f_max_velocity_v[ iAxis ] );
1586 }
1587 //----------------------------------------------------------------------
1588 
1589 
1590 std::vector<double> cSDH::GetAxisMaxAcceleration( std::vector<int> const& axes )
1591 {
1592  std::vector<double> rv( axes.size(), 0.0 );
1593 
1594  std::vector<int>::const_iterator ai;
1595  std::vector<double>::iterator vi;
1596  for( ai = axes.begin(), vi = rv.begin();
1597  ai != axes.end();
1598  ai++, vi++ )
1599  {
1600  CheckIndex( *ai, nb_all_axes, "axis" );
1601  *vi = uc_angle->ToExternal( f_max_acceleration_v[ *ai ] );
1602  }
1603 
1604  return rv;
1605 }
1606 //----------------------------------------------------------------------
1607 
1608 
1609 double cSDH::GetAxisMaxAcceleration( int iAxis )
1610 {
1611  CheckIndex( iAxis, nb_all_axes, "axis" );
1612 
1613  return uc_angle->ToExternal( f_max_acceleration_v[ iAxis ] );
1614 }
1615 //----------------------------------------------------------------------
1616 
1617 
1618 void cSDH::SetFingerEnable( std::vector<int> const& fingers, std::vector<double> const& states )
1619 {
1620 #if SDH_USE_VCC
1621  // VCC does not know nan to create a NAN value
1622  unsigned long nan[2]={0xffffffff, 0x7fffffff};
1623  std::vector<double> all_states( NUMBER_OF_AXES, *( double* )nan);
1624 #else
1625  std::vector<double> all_states( NUMBER_OF_AXES, nan("") );
1626 #endif
1627 
1628  std::vector<int>::const_iterator fi;
1629  std::vector<double>::const_iterator vi;
1630  for( fi = fingers.begin(), vi = states.begin();
1631  fi != fingers.end() && vi != states.end();
1632  fi++, vi++ )
1633  {
1634  CheckIndex( *fi, NUMBER_OF_FINGERS, "finger" );
1635 
1636  std::vector<int>::const_iterator fai;
1637  for( fai = finger_axis_index[ *fi ].begin();
1638  fai != finger_axis_index[ *fi ].end();
1639  fai++ )
1640  {
1641  if ( *fai == 0 && !SDH_ISNAN( all_states[ *fai ] ) && !SDH_ISNAN( *vi ) )
1642  {
1643  // special treatment for axis 0:
1644  // "or-together" all given states, so that axis
1645  // 0 is enabled if at least one finger using it
1646  // is eanabled.
1647 
1648  all_states[ *fai ] += *vi;
1649  }
1650  all_states[ *fai ] = *vi;
1651  }
1652  }
1653 
1654  // limit all_states[ 0 ] to [0.0 .. 1.0]
1655  if ( !SDH_ISNAN( all_states[ 0 ] ) )
1656  all_states[ 0 ] = ToRange( all_states[ 0 ], 0.0, 1.0 );
1657 
1658  SetAxisEnable( all_axes, all_states );
1659 }
1660 //----------------------------------------------------------------------
1661 
1662 
1663 void cSDH::SetFingerEnable( std::vector<int> const& fingers, std::vector<bool> const& states )
1664 {
1665  std::vector<double> dstates( states.size(), 0.0 );
1666  std::vector<bool>::const_iterator bi;
1667  std::vector<double>::iterator di;
1668  for ( bi = states.begin(), di = dstates.begin();
1669  bi != states.end() && di != dstates.end();
1670  bi++, di++ )
1671  *di = *bi;
1672 
1673  SetFingerEnable( fingers, dstates );
1674 
1675 }
1676 //----------------------------------------------------------------------
1677 
1678 
1679 void cSDH::SetFingerEnable( int iFinger, double state )
1680 {
1681  std::vector<int> axes;
1682 
1683  if ( iFinger == All )
1684  {
1685  axes = all_axes;
1686  }
1687  else
1688  {
1689  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1690  axes = finger_axis_index[ iFinger ];
1691  }
1692  // now axes is a list of all axis indices to access
1693 
1694  std::vector<double> states( axes.size(), state );
1695  // now states is a list of all axis states to set
1696 
1697  SetAxisValueVector( axes, states,
1700  &uc_identity,
1701  zeros_v, ones_v,
1702  "enable/disable" );
1703 }
1704 //----------------------------------------------------------------------
1705 
1706 
1707 void cSDH::SetFingerEnable( int iFinger, bool state )
1708 {
1709  SetFingerEnable( iFinger, state ? 1.0 : 0.0 );
1710 }
1711 //----------------------------------------------------------------------
1712 
1713 
1714 std::vector<double> cSDH::GetFingerEnable( std::vector<int> const& fingers )
1715 {
1716  std::vector<double> rv;
1717 
1718  std::vector<double> all_states = GetAxisEnable( all_axes );
1719 
1720 
1721  std::vector<int>::const_iterator fi;
1722 
1723  for( fi = fingers.begin();
1724  fi != fingers.end();
1725  fi++ )
1726  {
1727  CheckIndex( *fi, NUMBER_OF_FINGERS, "finger" );
1728 
1729  double finger_state = 0.0;
1730  std::vector<int>::const_iterator fai;
1731  for( fai = finger_axis_index[ *fi ].begin();
1732  fai != finger_axis_index[ *fi ].end();
1733  fai++ )
1734  {
1735  if ( IsVirtualAxis( *fai ) )
1736  finger_state += 1.0; // count virtual axes as enabled
1737  else
1738  finger_state += all_states[ *fai ];
1739  }
1740 
1741  rv.push_back( (finger_state == double(NUMBER_OF_AXES_PER_FINGER)) ? 1.0 : 0.0 );
1742  }
1743  return rv;
1744 }
1745 //----------------------------------------------------------------------
1746 
1747 
1748 double cSDH::GetFingerEnable( int iFinger )
1749 {
1750  return GetFingerEnable( std::vector<int>( 1, iFinger ) )[0];
1751 }
1752 //----------------------------------------------------------------------
1753 
1754 
1755 void cSDH::SetFingerTargetAngle( int iFinger, std::vector<double> const& angles )
1756 {
1757  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1758 
1759  SetAxisTargetAngle( finger_axis_index[ iFinger ], angles );
1760 }
1761 //----------------------------------------------------------------------
1762 
1763 
1764 void cSDH::SetFingerTargetAngle( int iFinger, double a0, double a1, double a2 )
1765 {
1766  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1767 
1768  std::vector<double> angles( NUMBER_OF_AXES_PER_FINGER, a0 );
1769  angles[1] = a1;
1770  angles[2] = a2;
1771 
1772  SetAxisTargetAngle( finger_axis_index[ iFinger ], angles );
1773 }
1774 //----------------------------------------------------------------------
1775 
1776 
1777 std::vector<double> cSDH::GetFingerTargetAngle( int iFinger )
1778 {
1779  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1780 
1781  return GetAxisTargetAngle( finger_axis_index[iFinger] );
1782 }
1783 //----------------------------------------------------------------------
1784 
1785 
1786 void cSDH::GetFingerTargetAngle( int iFinger, double& a0, double& a1, double& a2 )
1787 {
1788  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1789 
1790  std::vector<double> angles = GetAxisTargetAngle( finger_axis_index[iFinger] );
1791  a0 = angles[0];
1792  a1 = angles[1];
1793  a2 = angles[2];
1794 }
1795 //----------------------------------------------------------------------
1796 
1797 
1798 std::vector<double> cSDH::GetFingerActualAngle( int iFinger )
1799 {
1800  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1801 
1802  return GetAxisActualAngle( finger_axis_index[iFinger] );
1803 }
1804 //----------------------------------------------------------------------
1805 
1806 
1807 void cSDH::GetFingerActualAngle( int iFinger, double& a0, double& a1, double& a2 )
1808 {
1809  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1810 
1811  std::vector<double> angles = GetAxisActualAngle( finger_axis_index[iFinger] );
1812  a0 = angles[0];
1813  a1 = angles[1];
1814  a2 = angles[2];
1815 }
1816 //----------------------------------------------------------------------
1817 
1818 
1819 std::vector<double> cSDH::GetFingerMinAngle( int iFinger )
1820 {
1821  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1822 
1823  return GetAxisMinAngle( finger_axis_index[iFinger] );
1824 }
1825 //----------------------------------------------------------------------
1826 
1827 
1828 void cSDH::GetFingerMinAngle( int iFinger, double& a0, double& a1, double& a2 )
1829 {
1830  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1831 
1832  std::vector<double> angles = GetAxisMinAngle( finger_axis_index[iFinger] );
1833  a0 = angles[0];
1834  a1 = angles[1];
1835  a2 = angles[2];
1836 }
1837 //----------------------------------------------------------------------
1838 
1839 
1840 std::vector<double> cSDH::GetFingerMaxAngle( int iFinger )
1841 {
1842  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1843 
1844  return GetAxisMaxAngle(finger_axis_index[iFinger] );
1845 }
1846 //----------------------------------------------------------------------
1847 
1848 
1849 void cSDH::GetFingerMaxAngle( int iFinger, double& a0, double& a1, double& a2 )
1850 {
1851  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1852 
1853  std::vector<double> angles = GetAxisMaxAngle( finger_axis_index[iFinger] );
1854  a0 = angles[0];
1855  a1 = angles[1];
1856  a2 = angles[2];
1857 }
1858 //----------------------------------------------------------------------
1859 
1860 
1861 std::vector<double> cSDH::GetFingerXYZ( int iFinger, std::vector<double> const& angles )
1862 {
1863  CheckIndex( iFinger, NUMBER_OF_FINGERS, "finger" );
1864 
1865  // we need angle in radians
1866  std::vector<double> r_angles;
1867 
1868  if (uc_angle != &uc_angle_radians)
1869  {
1870  // angles is in other units
1871 
1872  // convert to internal (deg)
1873  std::vector<double> d_angles = uc_angle->ToInternal( angles );
1874  // convert to rad
1875  r_angles = map( DegToRad, d_angles );
1876  }
1877 
1878  // now r_angles is in rad
1879 
1880  // get xyz in internal unit (mm) and return converted to external unit
1881  return uc_position->ToExternal( _GetFingerXYZ( iFinger, r_angles ) );
1882 }
1883 //----------------------------------------------------------------------
1884 
1885 
1886 std::vector<double> cSDH::GetFingerXYZ( int iFinger, double a0, double a1, double a2 )
1887 {
1888  std::vector<double> a012;
1889  a012.push_back( a0 );
1890  a012.push_back( a1 );
1891  a012.push_back( a2 );
1892 
1893  return GetFingerXYZ( iFinger, a012 );
1894 }
1895 //----------------------------------------------------------------------
1896 
1897 
1898 double cSDH::MoveAxis( std::vector<int>const& axes, bool sequ )
1899 {
1900  // save currently set target axis angles of all axes in external units
1901  std::vector<double> t_angles = GetAxisTargetAngle( all_axes );
1902 
1903  // save current actual axis angles of all axes in external units
1904  std::vector<double> a_angles = GetAxisActualAngle( all_axes );
1905 
1906  // The angle positions might be reported slightly out of range if
1907  // an axis is close to the limit of its range. This is due to
1908  // inaccuracies of the absolute encoders. (Don't worry, this does
1909  // not influence repetitive accuracy, only absolute accuracy.)
1910 
1911  // Therefore we must limit the read actual angle back to the
1912  // allowed range to be able to send it back to the hand without
1913  // getting an exception:
1914  ToRange( a_angles, // a_angle is in external units
1915  uc_angle->ToExternal( f_min_angle_v ), // so convert min and max
1916  uc_angle->ToExternal( f_max_angle_v ) ); // to external untis too first
1917 
1918  //---------------------
1919  // generate new target axis angles:
1920  // - actual axis angle for not selected axes axes
1921  // - target axis angle for iAxiss axes
1922  //
1923  for ( std::vector<int>::const_iterator ai = axes.begin();
1924  ai != axes.end();
1925  ai++ )
1926  {
1927  CheckIndex( *ai, nb_all_axes, "axis" );
1928 
1929  if ( IsVirtualAxis( *ai ) )
1930  continue; // ignore virtual axes
1931 
1932  // set new target axis angles for the axes selected by the
1933  // given axes index vector \a axes into the actual axis
1934  // angles variable \a a_angles
1935  a_angles[ *ai ] = t_angles[ *ai ] ;
1936  }
1937  //---------------------
1938 
1939  // set modified actual axis angles as new target axis angles
1940  SetAxisTargetAngle( all_axes, a_angles );
1941  // and move there
1942  double t = comm_interface.m(sequ);
1943 
1944  // restore the saved target axis angles so that
1945  // previously set target axis angles for unselected axes remain
1946  // active
1947  if (sequ)
1948  {
1949  SetAxisTargetAngle( all_axes, t_angles );
1950  }
1951 
1952  return uc_time->ToExternal( t );
1953 }
1954 //----------------------------------------------------------------------
1955 
1956 
1957 double cSDH::MoveAxis( int iAxis, bool sequ )
1958 {
1959  if ( iAxis==All )
1960  return MoveAxis( all_axes, sequ );
1961  else
1962  return MoveAxis( std::vector<int>( 1, iAxis ), sequ );
1963 }
1964 //----------------------------------------------------------------------
1965 
1966 
1967 double cSDH::MoveFinger( std::vector<int>const& fingers, bool sequ )
1968 {
1969  // save currently set target axis angles of all axes in external units
1970  std::vector<double> t_angles = GetAxisTargetAngle( all_axes );
1971 
1972  // save current actual axis angles of all axes in external units
1973  std::vector<double> a_angles = GetAxisActualAngle( all_axes );
1974 
1975  // The angle positions might be reported slightly out of range if
1976  // an axis is close to the limit of its range. This is due to
1977  // inaccuracies of the absolute encoders. (Don't worry, this does
1978  // not influence repetitive accuracy, only absolute accuracy.)
1979 
1980  // Therefore we must limit the read actual angle back to the
1981  // allowd range to be able to send it back to the hand without
1982  // getting an exception:
1983  ToRange( a_angles, // a_angle is in external units
1984  uc_angle->ToExternal( f_min_angle_v ), // so convert min and max
1985  uc_angle->ToExternal( f_max_angle_v ) ); // to external untis too first
1986 
1987  //---------------------
1988  // generate new target axis angles:
1989  // - actual axis angle for not selected fingers axes
1990  // - target axis angle for iFingers axes
1991  //
1992  for ( std::vector<int>::const_iterator fi = fingers.begin();
1993  fi != fingers.end();
1994  fi++ )
1995  {
1996  CheckIndex( *fi, NUMBER_OF_FINGERS, "finger" );
1997 
1998  for ( std::vector<int>::const_iterator fai = finger_axis_index[ *fi ].begin();
1999  fai != finger_axis_index[ *fi ].end();
2000  fai++ )
2001  {
2002  if ( IsVirtualAxis( *fai ) )
2003  continue; // ignore virtual axes
2004 
2005  // set new target axis angles for the axes of finger *fi in actual axis angles
2006  a_angles[ *fai ] = t_angles[ *fai ] ;
2007  }
2008  }
2009  //---------------------
2010 
2011  // set modified actual axis angles as new target axis angles
2012  SetAxisTargetAngle( all_axes, a_angles );
2013  // and move there
2014  double t = comm_interface.m(sequ);
2015 
2016  // restore the saved target axis angles so that
2017  // previously set target axis angles for unselected fingers remain
2018  // active
2019  if (sequ)
2020  {
2021  SetAxisTargetAngle( all_axes, t_angles );
2022  }
2023 
2024  return uc_time->ToExternal( t );
2025 }
2026 //----------------------------------------------------------------------
2027 
2028 
2029 double cSDH::MoveFinger( int iFinger, bool sequ )
2030 {
2031  if ( iFinger==All )
2032  return MoveFinger( all_fingers, sequ );
2033  else
2034  return MoveFinger( std::vector<int>( 1, iFinger ), sequ );
2035 }
2036 //----------------------------------------------------------------------
2037 
2038 
2039 double cSDH::MoveHand( bool sequ )
2040 {
2041  return MoveFinger( all_fingers, sequ );
2042 }
2043 //----------------------------------------------------------------------
2044 
2045 
2047 {
2049 }
2050 //----------------------------------------------------------------------
2051 
2052 
2053 double cSDH::GripHand( eGraspId grip, double close, double velocity, bool sequ )
2054 {
2055  CheckRange( close, 0.0, 1.0, "open/close ratio" );
2056  CheckRange( velocity, 0.0, grip_max_velocity, "grip velocity" );
2057 
2058  double t0 = comm_interface.selgrip( grip, true ); // the selgrip must always be sequential
2059 
2060  double t1 = comm_interface.grip( close, uc_angular_velocity->ToInternal( velocity ), sequ );
2061 
2062  return uc_time->ToExternal( t0 + t1 );
2063 }
2064 //----------------------------------------------------------------------
2065 
2066 
2068 {
2069  // get the actual realease of the firmware (some code has to be version specific)
2070  release_firmware = GetInfo("release-firmware");
2071 
2072  // use the real axis velocity limits as reported by the firmware via the vlim command
2074 
2075  // GetAxisLimitVelocity() reports velocities in external units,
2076  // but f_max_velocity_v should store in internal units, so convert:
2077  std::vector<double>::iterator vi;
2078  for( vi = f_max_velocity_v.begin();
2079  vi != f_max_velocity_v.end();
2080  vi++ )
2081  {
2082  *vi = uc_angular_velocity->ToInternal( *vi );
2083  }
2084 
2085  // virtual axes will have maximum of all other velocities as limit velocity
2086  // Useful to make SetAxisTargetVelocity( All, x ) work,
2087  f_max_velocity_v.push_back( *max_element( f_max_velocity_v.begin(), f_max_velocity_v.end() ) );
2088 
2089 
2090  // use the real axis acceleration limits as reported by the firmware via the alim command
2092 
2093  // GetAxisLimitAcceleration() reports accelerations in external units,
2094  // but f_max_acceleration_v should store in internal units, so convert:
2095  for( vi = f_max_acceleration_v.begin();
2096  vi != f_max_acceleration_v.end();
2097  vi++ )
2098  {
2099  *vi = uc_angular_acceleration->ToInternal( *vi );
2100  }
2101 
2102  // virtual axes will have maximum of all other accelerations as limit acceleration
2103  // usefull to make SetAxisTargetAcceleration( All, x ) work,
2104  f_max_acceleration_v.push_back( *max_element( f_max_acceleration_v.begin(), f_max_acceleration_v.end() ) );
2105 
2107 }
2108 //----------------------------------------------------------------------
2109 
2110 
2112 {
2114  switch (controller)
2115  {
2116  case eCT_POSE:
2117  // in pose controller the velocities are always positive and thus the minimum is 0.0
2119  break;
2120 
2121  case eCT_VELOCITY:
2122  // no break here
2124  // in velocity based controllers the velocities can be positive or negative and thus the minimum is -maximum
2125  for ( int i = 0; i < nb_all_axes; i++ )
2126  {
2128  }
2129  break;
2130 
2131  case eCT_INVALID:
2132  case eCT_DIMENSION:
2133  assert( "controller invalid" == NULL );
2134  break;
2135  }
2136 
2137  cdbg << "AdjustLimits( " << GetStringFromControllerType( controller ) << " )\n";
2138  cdbg << " f_min_velocity_v = " << f_min_velocity_v << " ";
2139  cdbg << " f_min_acceleration_v = " << f_min_acceleration_v << "\n";
2140 }
2141 
2142 //======================================================================
2143 /*
2144  Here are some settings for the emacs/xemacs editor (and can be safely ignored)
2145  (e.g. to explicitely set C++ mode for *.h header files)
2146 
2147  Local Variables:
2148  mode:C++
2149  mode:ELSE
2150  End:
2151 */
2152 //======================================================================]
eAxisState
The state of an axis (see TPOSCON_STATE in global.h of the SDH firmware)
Definition: sdh.h:188
static cUnitConverter const uc_position_meter
Converter for position: external unit = meter.
Definition: sdh.h:259
void EmergencyStop(void)
Definition: sdh.cpp:925
A meta-value that means "access all possible values".
Definition: sdhbase.h:103
static cUnitConverter const uc_angle_degrees
Default converter for angles (internal unit == external unit): degrees.
Definition: sdh.h:220
Unit conversion class to convert values between physical unit systems.
std::vector< double > GetAxisTargetAcceleration(std::vector< int >const &axes)
Definition: sdh.cpp:1487
std::vector< int > all_temperature_sensors
A vector with indices of all temperature sensors.
Definition: sdh.h:526
std::vector< double > ones_v
Vector of nb_all_axes 1.0 values.
Definition: sdh.h:300
std::vector< double > _GetFingerXYZ(int fi, std::vector< double > r_angles)
Definition: sdh.cpp:544
This file contains the interface to class #SDH::cSDH, the end user class to access the SDH from a PC...
cUnitConverter const uc_identity
Identity converter (internal = external)
std::vector< double > GetAxisMaxVelocity(std::vector< int > const &axes)
Definition: sdh.cpp:1562
std::vector< double > GetFingerXYZ(int iFinger, std::vector< double > const &angles)
Definition: sdh.cpp:1861
The base class to control the SCHUNK Dexterous Hand.
Definition: sdhbase.h:97
Interface of auxilliary utility functions for SDHLibrary-CPP.
void CheckIndex(int index, int maxindex, char const *name="")
Check if index is in [0 .. maxindex-1] or All. Throw a cSDHErrorInvalidParameter exception if not...
Definition: sdhbase.cpp:169
void UpdateSettingsFromSDH()
Definition: sdh.cpp:2067
cSimpleVector temp(void)
Definition: sdhserial.cpp:1129
void SetVelocityProfile(eVelocityProfile velocity_profile)
Definition: sdh.cpp:986
void SetColor(char const *color)
Definition: dbg.h:170
void SetFingerTargetAngle(int iFinger, std::vector< double > const &angles)
Definition: sdh.cpp:1755
double Elapsed(void) const
Return time in seconds elapsed between the time stored in the object and now.
Definition: simpletime.h:115
eGraspId
The enum values of the known grasps.
Definition: sdhbase.h:162
static char const * GetFirmwareReleaseRecommended(void)
Definition: sdh.cpp:649
#define PROJECT_RELEASE
Release name of the whole software project (a.k.a. as the "version" of the project).
Definition: release.h:492
static cUnitConverter const uc_motor_current_milliampere
Converter for motor current: external unit = milli Ampere.
Definition: sdh.h:253
std::vector< double > GetAxisLimitAcceleration(std::vector< int > const &axes)
Definition: sdh.cpp:1374
Interface of class #SDH::cCANSerial_PEAK, class to access CAN bus via PEAK card on cygwin/linux...
std::vector< double > SetAxisTargetGetAxisActualAngle(std::vector< int > const &axes, std::vector< double > const &angles)
Definition: sdh.cpp:1239
int nb_all_axes
The number of all axes including virtual axes.
Definition: sdh.h:277
std::vector< double > f_max_motor_current_v
Maximum allowed motor currents (in internal units (Ampere)), including the virtual axis...
Definition: sdh.h:307
std::vector< double > GetFingerActualAngle(int iFinger)
Definition: sdh.cpp:1798
void WaitAxis(std::vector< int > const &axes, double timeout=-1.0)
Definition: sdh.cpp:1158
cSimpleVector pos(int axis=All, double *dummy=NULL)
Definition: sdhserial.cpp:1068
cSimpleVector v(int axis=All, double *velocity=NULL)
Definition: sdhserial.cpp:916
static cUnitConverter const uc_angular_velocity_degrees_per_second
Default converter for angular velocities (internal unit == external unit): degrees / second...
Definition: sdh.h:238
std::vector< double > GetTemperature(std::vector< int > const &sensors)
Definition: sdh.cpp:694
Endmarker and dimension.
Definition: sdhbase.h:186
A simple vector implementation.
Definition: simplevector.h:91
cSerialBase * com
Definition: sdh.h:357
velocity controller, velocities of axes are controlled independently (not implemented in SDH firmware...
Definition: sdhbase.h:180
char * soc(void)
Definition: sdhserial.cpp:1188
void SetAxisEnable(std::vector< int > const &axes, std::vector< double > const &states)
Definition: sdh.cpp:1055
#define NULL
Definition: getopt1.c:56
pSetFunction GetMotorCurrentModeFunction(eMotorCurrentMode mode)
Definition: sdh.cpp:514
void SetFlag(bool flag)
Definition: dbg.h:149
std::vector< int > finger_number_of_axes
Mapping of finger index to number of real axes of fingers:
Definition: sdh.h:280
Interface of class #SDH::cTCPSerial, class to access TCP port cygwin/linux.
char * ver_date(void)
Definition: sdhserial.cpp:1164
eMotorCurrentMode
the motor current can be set specifically for these modes:
Definition: sdh.h:177
void Stop(void)
Definition: sdh.cpp:939
static cUnitConverter const uc_time_milliseconds
Converter for times: external unit = milliseconds.
Definition: sdh.h:229
static cUnitConverter const uc_time_seconds
Default converter for times (internal unit == external unit): seconds.
Definition: sdh.h:226
velocity controller with acceleration ramp, velocities and accelerations of axes are controlled indep...
Definition: sdhbase.h:181
cSimpleVector vel(int axis=All, double *dummy=NULL)
Definition: sdhserial.cpp:1096
std::vector< double > zeros_v
Vector of nb_all_axes 0.0 values.
Definition: sdh.h:297
double m(bool sequ)
Definition: sdhserial.cpp:985
std::vector< double > f_max_velocity_v
Maximum allowed axis velocity (in internal units (degrees/second)), including the virtual axis...
Definition: sdh.h:321
virtual const char * what() const
Interface of class #SDH::cCANSerial_ESD, class to access CAN bus via ESD card on cygwin/linux.
cSimpleVector tvav(int axis=All, double *velocity=NULL)
Definition: sdhserial.cpp:927
void Open(cSerialBase *_com)
Definition: sdhserial.cpp:293
int NUMBER_OF_VIRTUAL_AXES
The number of virtual axes.
Definition: sdh.h:274
const cUnitConverter * uc_position
unit converter for position: default = #SDH::cSDH::uc_position_millimeter
Definition: sdh.h:582
cSimpleVector ihold(int axis=All, double *limit=NULL)
Definition: sdhserial.cpp:1222
static char const * GetLibraryName(void)
Definition: sdh.cpp:633
cSimpleVector a(int axis=All, double *acceleration=NULL)
Definition: sdhserial.cpp:952
cSimpleVector temp_electronics(void)
Definition: sdhserial.cpp:1141
double MoveFinger(std::vector< int >const &fingers, bool sequ=true)
Definition: sdh.cpp:1967
std::vector< double > GetAxisTargetVelocity(std::vector< int > const &axes)
Definition: sdh.cpp:1329
cSimpleVector vlim(int axis=All, double *dummy=NULL)
Definition: sdhserial.cpp:938
cSimpleVector(cSDHSerial::* pSetFunction)(int, double *)
Type of a pointer to a "set-axis-values" function like cSDHSerial::p, cSDHSerial::pos, ..., cSDHSerial::igrip, cSDHSerial::ihold or cSDHSerial::ilim.
Definition: sdhserial.h:883
Tp map(Function f, Tp sequence)
Definition: util.h:267
eVelocityProfile
An enum for all possible SDH internal velocity profile types.
Definition: sdhbase.h:191
char * soc_date(void)
Definition: sdhserial.cpp:1196
bool CheckFirmwareRelease(void)
Definition: sdh.cpp:655
std::vector< double > f_min_angle_v
Minimum allowed axis angles (in internal units (degrees)), including the virtual axis.
Definition: sdh.h:312
double selgrip(eGraspId grip, bool sequ)
Definition: sdhserial.cpp:1229
std::vector< int > ToIndexVector(int index, std::vector< int > &all_replacement, int maxindex, char const *name)
Definition: sdh.cpp:492
eControllerType con(eControllerType controller)
Definition: sdhserial.cpp:1049
double l2
length of limb 2 (distal joint to fingertip) in mm
Definition: sdh.h:343
const cUnitConverter * uc_angular_acceleration
unit convert for (axis) angular accelerations: default = #SDH::cSDH::uc_angular_acceleration_degrees_...
Definition: sdh.h:566
double DegToRad(double d)
Definition: util.cpp:143
cSimpleVector rvel(int axis, double *dummy=NULL)
Definition: sdhserial.cpp:1107
Endmarker and Dimension.
Definition: sdh.h:183
std::vector< double > GetFingerMinAngle(int iFinger)
Definition: sdh.cpp:1819
cSimpleVector ilim(int axis=All, double *limit=NULL)
Definition: sdhserial.cpp:851
The motor currents used after "gripping" with a GripHand() command (i.e. "holding") ...
Definition: sdh.h:181
virtual ~cSDH()
Definition: sdh.cpp:475
eVelocityProfile GetVelocityProfile(void)
Definition: sdh.cpp:993
the goal position has not been reached yet
Definition: sdh.h:191
coordinated position controller (position per axis => "pose controller"), all axes start and stop mov...
Definition: sdhbase.h:179
std::vector< double > GetAxisLimitVelocity(std::vector< int > const &axes)
Definition: sdh.cpp:1348
axis is in speed mode
Definition: sdh.h:192
void SetAxisTargetAcceleration(std::vector< int >const &axes, std::vector< double >const &accelerations)
Definition: sdh.cpp:1457
std::vector< double > GetFingerTargetAngle(int iFinger)
Definition: sdh.cpp:1777
cSimpleVector tpap(int axis=All, double *angle=NULL)
Definition: sdhserial.cpp:974
cSimpleVector p(int axis=All, double *angle=NULL)
Definition: sdhserial.cpp:963
static cUnitConverter const uc_angle_radians
Converter for angles: external unit = radians.
Definition: sdh.h:223
void OpenTCP(char const *_tcp_adr="192.168.1.1", int _tcp_port=23, double _timeout=0.0)
Definition: sdh.cpp:866
int NUMBER_OF_AXES
The number of axes.
Definition: sdhbase.h:297
int CompareReleases(char const *rev1, char const *rev2)
compare release strings
Definition: util.cpp:236
double GripHand(eGraspId grip, double close, double velocity, bool sequ=true)
Definition: sdh.cpp:2053
virtual bool IsOpen(void)
Definition: sdh.cpp:917
std::vector< double > f_min_acceleration_v
Minimum allowed axis acceleration (in internal units (degrees/(second * second))), including the virtual axis.
Definition: sdh.h:324
void stop(void)
Definition: sdhserial.cpp:1023
char * sn(void)
Definition: sdhserial.cpp:1180
int GetFingerNumberOfAxes(int iFinger)
Definition: sdh.cpp:608
int NUMBER_OF_TEMPERATURE_SENSORS
The number of temperature sensors.
Definition: sdhbase.h:303
const cUnitConverter * uc_time
unit convert for times: default = uc_time_seconds
Definition: sdh.h:570
const cUnitConverter * uc_angular_velocity
unit convert for (axis) angular velocities: default = #SDH::cSDH::uc_angular_velocity_degrees_per_sec...
Definition: sdh.h:562
Base class for exceptions in the SDHLibrary-CPP.
Definition: sdhexception.h:132
The motor currents used while "moving" with a MoveHand() or MoveFinger() command. ...
Definition: sdh.h:179
static cUnitConverter const uc_angular_velocity_radians_per_second
Converter for angular velocieties: external unit = radians/second.
Definition: sdh.h:241
std::vector< double > SetAxisValueVector(std::vector< int > const &axes, std::vector< double > const &values, pSetFunction ll_set, pGetFunction ll_get, cUnitConverter const *uc, std::vector< double > const &min_values, std::vector< double > const &max_values, char const *name)
Definition: sdh.cpp:134
void SetFingerEnable(std::vector< int > const &fingers, std::vector< double > const &states)
Definition: sdh.cpp:1618
double d
Definition: sdh.h:346
virtual bool IsOpen(void)
Definition: sdhserial.cpp:381
std::vector< eAxisState > GetAxisActualState(std::vector< int > const &axes)
Definition: sdh.cpp:1127
void SetController(cSDHBase::eControllerType controller)
Definition: sdh.cpp:946
int NUMBER_OF_FINGERS
The number of fingers.
Definition: sdhbase.h:300
std::vector< double > f_zeros_v
Vector of 3 epsilon values.
Definition: sdh.h:290
Interface of auxilliary utility functions for SDHLibrary-CPP.
eControllerType controller_type
cached value of the axis controller type
Definition: sdh.h:4061
The motor currents used while "gripping" with a GripHand() command.
Definition: sdh.h:180
static cUnitConverter const uc_position_millimeter
Default converter for position (internal unit == external unit): millimeter.
Definition: sdh.h:256
invalid controller_type (needed for cSDHSerial::con() to indicate "read current controller type") ...
Definition: sdhbase.h:178
void Close(bool leave_enabled=false)
Definition: sdh.cpp:888
const cUnitConverter * uc_motor_current
unit converter for motor curent: default = #SDH::cSDH::uc_motor_current_ampere
Definition: sdh.h:578
void SetAxisTargetVelocity(std::vector< int > const &axes, std::vector< double > const &velocities)
Definition: sdh.cpp:1289
std::vector< int > all_fingers
A vector with indices of all fingers (in natural order)
Definition: sdh.h:523
std::vector< double > GetAxisValueVector(std::vector< int > const &axes, pGetFunction ll_get, cUnitConverter const *uc, char const *name)
Definition: sdh.cpp:251
const cUnitConverter * uc_angle
unit convert for (axis) angles: default = #SDH::cSDH::uc_angle_degrees
Definition: sdh.h:558
void CheckRange(double value, double minvalue, double maxvalue, char const *name="")
Check if value is in [minvalue .. maxvalue]. Throw a cSDHErrorInvalidParameter exception if not...
Definition: sdhbase.cpp:177
Low-level communication class to access a serial port on Cygwin and Linux.
Definition: rs232-cygwin.h:84
eControllerType
An enum for all possible SDH internal controller types (order must match that in the SDH firmware in ...
Definition: sdhbase.h:176
Low-level communication class to access a CAN port.
Definition: tcpserial.h:88
std::vector< int > all_real_axes
A vector with indices of all real axes (in natural order), excluding the virtual axis.
Definition: sdh.h:520
cDBG cdbg
debug stream to print colored debug messages
Definition: sdhbase.h:279
void SetAxisTargetAngle(std::vector< int > const &axes, std::vector< double > const &angles)
Definition: sdh.cpp:1209
std::vector< double > GetFingerEnable(std::vector< int > const &fingers)
Definition: sdh.cpp:1714
char * ver(void)
Definition: sdhserial.cpp:1156
double ToExternal(double internal) const
cSDHSerial comm_interface
The object to interface with the SDH attached via serial RS232 or CAN or TCP.
Definition: sdh.h:362
cSDH(bool _use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
Constructor of cSDH class.
Definition: sdh.cpp:298
int GetFingerAxisIndex(int iFinger, int iFingerAxis)
Definition: sdh.cpp:616
int NUMBER_OF_AXES_PER_FINGER
The number of axis per finger (for finger 1 this includes the "virtual" base axis) ...
Definition: sdh.h:270
static cUnitConverter const uc_temperature_celsius
Default converter for temparatures (internal unit == external unit): degrees celsius.
Definition: sdh.h:232
char const * GetInfo(char const *what)
Definition: sdh.cpp:661
cSimpleVector state(int axis=All, double *dummy=NULL)
Definition: sdhserial.cpp:1118
double ToRange(double v, double min, double max)
Definition: util.cpp:97
std::vector< double > GetAxisMinAngle(std::vector< int > const &axes)
Definition: sdh.cpp:1506
void OpenRS232(int _port=0, unsigned long _baudrate=115200, double _timeout=-1, char const *_device_format_string="/dev/ttyS%d")
Definition: sdh.cpp:733
#define USING_NAMESPACE_SDH
double(cUnitConverter::* pDoubleUnitConverterFunction)(double) const
Type of a pointer to a function like &#39;double SDH::cUnitConverter::ToExternal( double ) const&#39; or &#39;dou...
std::vector< double > GetAxisTargetAngle(std::vector< int > const &axes)
Definition: sdh.cpp:1251
std::vector< double > f_max_acceleration_v
Maximum allowed axis acceleration (in internal units (degrees/(second * second))), including the virtual axis.
Definition: sdh.h:327
double grip(double close, double velocity, bool sequ)
Definition: sdhserial.cpp:1277
int all_axes_used
Bit field with the bits for all axes set.
Definition: sdhbase.h:306
double grip_max_velocity
Maximum allowed grip velocity (in internal units (degrees/second))
Definition: sdh.h:330
cSimpleVector alim(int axis=All, double *dummy=NULL)
Definition: sdhserial.cpp:945
std::vector< std::vector< int > > finger_axis_index
Mapping of finger index, finger axis index to axis index:
Definition: sdh.h:284
void OpenCAN_ESD(int _net=0, unsigned long _baudrate=1000000, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42)
Definition: sdh.cpp:754
std::vector< double > f_max_angle_v
Maximum allowed axis angles (in internal units (degrees)), including the virtual axis.
Definition: sdh.h:315
std::vector< double > GetAxisMotorCurrent(std::vector< int > const &axes, eMotorCurrentMode mode=eMCM_MOVE)
Definition: sdh.cpp:1034
std::vector< double > GetAxisEnable(std::vector< int > const &axes)
Definition: sdh.cpp:1108
std::vector< double > f_min_velocity_v
Minimum allowed axis velocity (in internal units (degrees/second)), including the virtual axis...
Definition: sdh.h:318
Interface of class #SDH::cRS232, a class to access serial RS232 port on cygwin/linux.
static char const * GetLibraryRelease(void)
Definition: sdh.cpp:626
static cUnitConverter const uc_angular_acceleration_degrees_per_second_squared
Default converter for angular accelerations (internal unit == external unit): degrees / second...
Definition: sdh.h:244
double MoveAxis(std::vector< int >const &axes, bool sequ=true)
Definition: sdh.cpp:1898
Derived exception class for exceptions related to communication between the SDHLibrary and the SDH...
Definition: sdhexception.h:206
cSimpleVector igrip(int axis=All, double *limit=NULL)
Definition: sdhserial.cpp:1215
This file contains settings to make the SDHLibrary compile on differen systems:
static cUnitConverter const uc_motor_current_ampere
Default converter for motor current (internal unit == external unit): Ampere.
Definition: sdh.h:250
static cUnitConverter const uc_angular_acceleration_radians_per_second_squared
Converter for angular velocieties: external unit = radians/second.
Definition: sdh.h:247
double h
Definition: sdh.h:349
static char const * GetStringFromControllerType(eControllerType controller_type)
Return a ptr to a (static) string describing controller type controller_Type.
Definition: sdhbase.cpp:216
Low-level communication class to access a CAN port from company PEAK (http://www.peak-system.com)
Very simple class to measure elapsed time.
Definition: simpletime.h:84
char const * GetFirmwareRelease(void)
Definition: sdh.cpp:640
std::string release_firmware
string containing the SDH firmware release of the attaced SDH (something like "0.0.2.7")
Definition: sdh.h:4058
eVelocityProfile vp(eVelocityProfile velocity_profile=eVP_INVALID)
Definition: sdhserial.cpp:1030
std::vector< double > GetAxisActualAngle(std::vector< int > const &axes)
Definition: sdh.cpp:1270
std::vector< double > GetAxisMaxAngle(std::vector< int > const &axes)
Definition: sdh.cpp:1534
void AdjustLimits(cSDHBase::eControllerType controller)
Definition: sdh.cpp:2111
double MoveHand(bool sequ=true)
Definition: sdh.cpp:2039
void Close()
Definition: sdhserial.cpp:374
std::vector< double > f_min_motor_current_v
Minimum allowed motor currents (in internal units (Ampere)), including the virtual axis...
Definition: sdh.h:304
std::vector< double > f_ones_v
Vector of 3 1.0 values.
Definition: sdh.h:293
#define FIRMWARE_RELEASE_RECOMMENDED
Definition: release.h:57
eControllerType GetController(void)
Definition: sdh.cpp:971
bool IsVirtualAxis(int iAxis)
Return true if index iAxis refers to a virtual axis.
Definition: sdh.cpp:126
void UseDegrees(void)
Definition: sdh.cpp:594
#define PROJECT_DATE
Date of the release of the software project.
Definition: release.h:499
std::vector< double > SetAxisTargetGetAxisActualVelocity(std::vector< int > const &axes, std::vector< double > const &velocities)
Definition: sdh.cpp:1318
double ToInternal(double external) const
std::vector< double > GetAxisReferenceVelocity(std::vector< int >const &axes)
Definition: sdh.cpp:1438
std::vector< double > GetAxisActualVelocity(std::vector< int >const &axes)
Definition: sdh.cpp:1419
This file contains nothing but C/C++ defines with the name of the project itself (PROJECT_NAME) and t...
Class for short, fixed maximum length text messages.
Definition: sdhexception.h:77
Derived exception class for exceptions related to invalid parameters.
Definition: sdhbase.h:78
void SetAxisMotorCurrent(std::vector< int > const &axes, std::vector< double > const &motor_currents, eMotorCurrentMode mode=eMCM_MOVE)
Definition: sdh.cpp:1002
cSimpleVector power(int axis=All, double *flag=NULL)
Definition: sdhserial.cpp:862
Low-level communication class to access a CAN port from company ESD (http://www.esd.eu/)
Definition: canserial-esd.h:97
double l1
length of limb 1 (proximal joint to distal joint) in mm
Definition: sdh.h:340
void PDM(char const *fmt,...) SDH__attribute__((format(printf
void OpenCAN_PEAK(unsigned long _baudrate=1000000, double _timeout=0.0, Int32 _id_read=43, Int32 _id_write=42, const char *_device="/dev/pcanusb0")
Definition: sdh.cpp:810
double eps
epsilon value (max absolute deviation of reported values from actual hardware values) (needed since S...
Definition: sdhbase.h:315
const cUnitConverter * uc_temperature
unit convert for temperatures: default = #SDH::cSDH::uc_temperature_celsius
Definition: sdh.h:574
Implementation of class #SDH::cRS232, a class to access serial RS232 port with VCC compiler on Window...
std::vector< std::vector< double > > offset
Definition: sdh.h:355
cSimpleVector(cSDHSerial::* pGetFunction)(int, double *)
Type of a pointer to a "get-axis-values" function like cSDHSerial::p, cSDHSerial::pos, ..., cSDHSerial::igrip, cSDHSerial::ihold or cSDHSerial::ilim.
Definition: sdhserial.h:886
int32_t Int32
signed integer, size 4 Byte (32 Bit)
Definition: basisdef.h:64
cDBG dbg
A stream object to print colored debug messages.
Definition: serialbase.h:227
std::vector< int > all_axes
A vector with indices of all axes (in natural order), including the virtual axis. ...
Definition: sdh.h:517
NAMESPACE_SDH_START typedef void * tDeviceHandle
generic device handle for CAN devices
Definition: serialbase.h:64
static cUnitConverter const uc_temperature_fahrenheit
Converter for temperatures: external unit = degrees fahrenheit.
Definition: sdh.h:235
double GetGripMaxVelocity(void)
Definition: sdh.cpp:2046
char * id(void)
Definition: sdhserial.cpp:1172
void UseRadians(void)
Definition: sdh.cpp:580
int debug_level
debug level of this object
Definition: sdhbase.h:282
#define PROJECT_NAME
Name of the software project.
Definition: release.h:51
std::vector< double > GetAxisMaxAcceleration(std::vector< int > const &axes)
Definition: sdh.cpp:1590
std::vector< double > GetFingerMaxAngle(int iFinger)
Definition: sdh.cpp:1840


sdhlibrary_cpp
Author(s): Dirk Osswald
autogenerated on Mon Feb 28 2022 23:41:51