00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00047
00048
00049 #ifndef SDH_h_
00050 #define SDH_h_
00051
00052 #include "sdhlibrary_settings.h"
00053
00054 #if SDH_USE_VCC
00055 # pragma warning(disable : 4996)
00056 #else
00057
00058 extern "C" {
00059 # include <stdint.h>
00060 }
00061 #endif
00062
00063 #include "basisdef.h"
00064
00065
00066
00067
00068
00069 #include <vector>
00070 #include <string>
00071
00072
00073
00074
00075
00076 #include "sdhbase.h"
00077 #include "sdhserial.h"
00078 #include "unit_converter.h"
00079 #include "serialbase.h"
00080
00081
00082
00083
00084
00085 #if SDH_USE_NAMESPACE
00086
00093 #endif
00094
00095 NAMESPACE_SDH_START
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 #if SDH_USE_VCC
00117
00118
00119
00120
00121 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<int>;
00122 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<int, std::allocator<int> >;
00123
00124 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<std::vector<int> >;
00125 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<std::vector<int>, std::allocator<std::vector<int> > >;
00126
00127 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<double>;
00128 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<double, std::allocator<double> >;
00129
00130 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::allocator<std::vector<double> >;
00131 VCC_EXPORT_TEMPLATE template class VCC_EXPORT std::vector<std::vector<double>, std::allocator<std::vector<double> > >;
00132 #endif
00133
00134
00135
00189 class VCC_EXPORT cSDH : public cSDHBase
00190 {
00191 public:
00192
00194 enum eMotorCurrentMode
00195 {
00196 eMCM_MOVE = 0,
00197 eMCM_GRIP = 1,
00198 eMCM_HOLD = 2,
00199
00200 eMCM_DIMENSION
00201 };
00202
00203
00205 enum eAxisState
00206 {
00207 eAS_IDLE = 0,
00208 eAS_POSITIONING,
00209 eAS_SPEED_MODE,
00210 eAS_NOT_INITIALIZED,
00211 eAS_CW_BLOCKED,
00212 eAS_CCW_BLOCKED,
00213 eAS_DISABLED,
00214 eAS_LIMITS_REACHED,
00215
00216 eAS_DIMENSION
00217 };
00218
00219
00220
00236
00237 static cUnitConverter const uc_angle_degrees;
00238
00240 static cUnitConverter const uc_angle_radians;
00241
00243 static cUnitConverter const uc_time_seconds;
00244
00246 static cUnitConverter const uc_time_milliseconds;
00247
00249 static cUnitConverter const uc_temperature_celsius;
00250
00252 static cUnitConverter const uc_temperature_fahrenheit;
00253
00255 static cUnitConverter const uc_angular_velocity_degrees_per_second;
00256
00258 static cUnitConverter const uc_angular_velocity_radians_per_second;
00259
00261 static cUnitConverter const uc_angular_acceleration_degrees_per_second_squared;
00262
00264 static cUnitConverter const uc_angular_acceleration_radians_per_second_squared;
00265
00267 static cUnitConverter const uc_motor_current_ampere;
00268
00270 static cUnitConverter const uc_motor_current_milliampere;
00271
00273 static cUnitConverter const uc_position_millimeter;
00274
00276 static cUnitConverter const uc_position_meter;
00277
00278
00279
00281
00282 protected:
00283
00284
00285
00287 int NUMBER_OF_AXES_PER_FINGER;
00288
00289
00291 int NUMBER_OF_VIRTUAL_AXES;
00292
00294 int nb_all_axes;
00295
00297 std::vector<int> finger_number_of_axes;
00298
00299
00301 std::vector<std::vector<int> > finger_axis_index;
00302
00304
00305
00307 std::vector<double> f_zeros_v;
00308
00310 std::vector<double> f_ones_v;
00311
00312
00314 std::vector<double> zeros_v;
00315
00317 std::vector<double> ones_v;
00318
00319
00321 std::vector<double> f_min_motor_current_v;
00322
00324 std::vector<double> f_max_motor_current_v;
00325
00326
00327
00329 std::vector<double> f_min_angle_v;
00330
00332 std::vector<double> f_max_angle_v;
00333
00335 std::vector<double> f_min_velocity_v;
00336
00338 std::vector<double> f_max_velocity_v;
00339
00341 std::vector<double> f_min_acceleration_v;
00342
00344 std::vector<double> f_max_acceleration_v;
00345
00347 double grip_max_velocity;
00348
00356
00357 double l1;
00358
00360 double l2;
00361
00362
00363 double d;
00364
00365
00366 double h;
00367
00372 std::vector<std::vector<double> > offset;
00373
00374 cSerialBase* com;
00375
00376
00377 public:
00379 cSDHSerial comm_interface;
00380
00382 virtual void SetDebugOutput(std::ostream* debuglog)
00383 {
00384 cSDHBase::SetDebugOutput(debuglog);
00385 comm_interface.SetDebugOutput(debuglog);
00386 }
00387
00388 protected:
00389
00390
00398
00446 std::vector<double> SetAxisValueVector(std::vector<int> const& axes,
00447 std::vector<double> const& values,
00448 pSetFunction ll_set,
00449 pGetFunction ll_get,
00450 cUnitConverter const* uc,
00451 std::vector<double> const& min_values,
00452 std::vector<double> const& max_values,
00453 char const* name)
00454 throw (cSDHLibraryException*);
00455
00456
00457
00478 std::vector<double> GetAxisValueVector(std::vector<int> const& axes,
00479 pGetFunction ll_get,
00480 cUnitConverter const* uc,
00481 char const* name)
00482 throw (cSDHLibraryException*);
00483
00484
00500 std::vector<int> ToIndexVector(int index, std::vector<int>& all_replacement, int maxindex, char const* name)
00501 throw (cSDHLibraryException*);
00502
00503
00504
00510 pSetFunction GetMotorCurrentModeFunction(eMotorCurrentMode mode)
00511 throw(cSDHLibraryException*);
00512
00513
00514
00518 std::vector<double> _GetFingerXYZ(int fi, std::vector<double> r_angles)
00519 throw(cSDHLibraryException*);
00520
00521
00522
00524
00525
00526
00527 public:
00528
00536
00537 std::vector<int> all_axes;
00538
00540 std::vector<int> all_real_axes;
00541
00543 std::vector<int> all_fingers;
00544
00546 std::vector<int> all_temperature_sensors;
00547
00548
00550
00551
00552
00553
00577
00578 const cUnitConverter* uc_angle;
00579
00580
00582 const cUnitConverter* uc_angular_velocity;
00583
00584
00586 const cUnitConverter* uc_angular_acceleration;
00587
00588
00590 const cUnitConverter* uc_time;
00591
00592
00594 const cUnitConverter* uc_temperature;
00595
00596
00598 const cUnitConverter* uc_motor_current;
00599
00600
00602 const cUnitConverter* uc_position;
00603
00604
00605
00607
00608
00609
00610
00694 cSDH(bool _use_radians = false, bool _use_fahrenheit = false, int _debug_level = 0);
00695
00696
00697
00705 virtual ~cSDH();
00706
00707
00708
00716
00718 bool IsVirtualAxis(int iAxis)
00719 throw (cSDHLibraryException*);
00720
00721
00739 void UseRadians(void);
00740
00741
00742
00762 void UseDegrees(void);
00763
00764
00765
00785 int GetFingerNumberOfAxes(int iFinger)
00786 throw (cSDHLibraryException*);
00787
00788
00789
00813 int GetFingerAxisIndex(int iFinger, int iFingerAxis)
00814 throw (cSDHLibraryException*);
00815
00816
00817
00832 static char const* GetLibraryRelease(void);
00833
00834
00835
00850 static char const* GetLibraryName(void);
00851
00852
00853
00871 char const* GetFirmwareRelease(void)
00872 throw (cSDHLibraryException*);
00873
00874
00890 static char const* GetFirmwareReleaseRecommended(void);
00891
00892
00922 bool CheckFirmwareRelease(void)
00923 throw (cSDHLibraryException*);
00924
00925
00954 char const* GetInfo(char const* what)
00955 throw (cSDHLibraryException*);
00956
00957
01010 std::vector<double> GetTemperature(std::vector<int> const& sensors)
01011 throw (cSDHLibraryException*);
01012
01013
01014
01019 double GetTemperature(int iSensor)
01020 throw (cSDHLibraryException*);
01021
01022
01023
01025
01026
01027
01028
01036
01071 void OpenRS232(int _port = 0, unsigned long _baudrate = 115200, double _timeout = -1, char const* _device_format_string = "/dev/ttyS%d")
01072 throw (cSDHLibraryException*);
01073
01074
01102 void OpenCAN_ESD(int _net = 0, unsigned long _baudrate = 1000000, double _timeout = 0.0, Int32 _id_read = 43, Int32 _id_write = 42)
01103 throw (cSDHLibraryException*);
01104
01105
01134 void OpenCAN_ESD(tDeviceHandle _ntcan_handle, double _timeout = 0.0, Int32 _id_read = 43, Int32 _id_write = 42)
01135 throw (cSDHLibraryException*);
01136
01137
01165 void OpenCAN_PEAK(unsigned long _baudrate = 1000000, double _timeout = 0.0, Int32 _id_read = 43, Int32 _id_write = 42, const char *_device = "/dev/pcanusb0")
01166 throw (cSDHLibraryException*);
01167
01168
01197 void OpenCAN_PEAK(tDeviceHandle _handle, double _timeout = 0.0, Int32 _id_read = 43, Int32 _id_write = 42)
01198 throw (cSDHLibraryException*);
01199
01219 void OpenTCP(char const* _tcp_adr = "192.168.1.1", int _tcp_port = 23, double _timeout = 0.0)
01220 throw (cSDHLibraryException*);
01221
01222
01253 void Close(bool leave_enabled = false)
01254 throw (cSDHLibraryException*);
01255
01256
01260 virtual bool IsOpen(void)
01261 throw ();
01262
01263
01264
01266
01267
01268
01269
01277
01299 void EmergencyStop(void)
01300 throw (cSDHLibraryException*);
01301
01302
01303
01334 void Stop(void)
01335 throw (cSDHLibraryException*);
01336
01337
01338
01339
01340
01341
01342
01343
01393 void SetController(cSDHBase::eControllerType controller)
01394 throw(cSDHLibraryException*);
01395
01396
01418 eControllerType GetController(void)
01419 throw (cSDHLibraryException*);
01420
01421
01442 void SetVelocityProfile(eVelocityProfile velocity_profile)
01443 throw (cSDHLibraryException*);
01444
01445
01446
01465 eVelocityProfile GetVelocityProfile(void)
01466 throw (cSDHLibraryException*);
01467
01468
01469
01471
01472
01473
01474
01482
01555 void SetAxisMotorCurrent(std::vector<int> const& axes, std::vector<double> const& motor_currents, eMotorCurrentMode mode = eMCM_MOVE)
01556 throw (cSDHLibraryException*);
01557
01558
01559
01566 void SetAxisMotorCurrent(int iAxis, double motor_current, eMotorCurrentMode mode = eMCM_MOVE)
01567 throw (cSDHLibraryException*);
01568
01569
01570
01622 std::vector<double> GetAxisMotorCurrent(std::vector<int> const& axes, eMotorCurrentMode mode = eMCM_MOVE)
01623 throw (cSDHLibraryException*);
01624
01625
01626
01631 double GetAxisMotorCurrent(int iAxis, eMotorCurrentMode mode = eMCM_MOVE)
01632 throw (cSDHLibraryException*);
01633
01634
01635
01698 void SetAxisEnable(std::vector<int> const& axes, std::vector<double> const& states)
01699 throw (cSDHLibraryException*);
01700
01701
01702
01709 void SetAxisEnable(int iAxis = All, double state = 1.0)
01710 throw (cSDHLibraryException*);
01711
01712
01713
01718 void SetAxisEnable(std::vector<int> const& axes, std::vector<bool> const& states)
01719 throw (cSDHLibraryException*);
01720
01721
01722
01729 void SetAxisEnable(int iAxis = All, bool state = true)
01730 throw (cSDHLibraryException*);
01731
01732
01733
01782 std::vector<double> GetAxisEnable(std::vector<int> const& axes)
01783 throw (cSDHLibraryException*);
01784
01785
01786
01791 double GetAxisEnable(int iAxis)
01792 throw (cSDHLibraryException*);
01793
01794
01795
01841 std::vector<eAxisState> GetAxisActualState(std::vector<int> const& axes)
01842 throw (cSDHLibraryException*);
01843
01844
01845
01850 eAxisState GetAxisActualState(int iAxis)
01851 throw (cSDHLibraryException*);
01852
01853
01854
01977 void WaitAxis(std::vector<int> const& axes, double timeout = -1.0)
01978 throw (cSDHLibraryException*);
01979
01980
01981
01982
01989 void WaitAxis(int iAxis, double timeout = -1.0)
01990 throw (cSDHLibraryException*);
01991
01992
01993
02067 void SetAxisTargetAngle(std::vector<int> const& axes, std::vector<double> const& angles)
02068 throw (cSDHLibraryException*);
02069
02070
02071
02078 void SetAxisTargetAngle(int iAxis, double angle)
02079 throw (cSDHLibraryException*);
02080
02081
02082
02147 std::vector<double> SetAxisTargetGetAxisActualAngle(std::vector<int> const& axes, std::vector<double> const& angles)
02148 throw (cSDHLibraryException*);
02149
02150
02151
02200 std::vector<double> GetAxisTargetAngle(std::vector<int> const& axes)
02201 throw (cSDHLibraryException*);
02202
02203
02204
02210 double GetAxisTargetAngle(int iAxis)
02211 throw (cSDHLibraryException*);
02212
02213
02214
02263 std::vector<double> GetAxisActualAngle(std::vector<int> const& axes)
02264 throw (cSDHLibraryException*);
02265
02266
02267
02273 double GetAxisActualAngle(int iAxis)
02274 throw (cSDHLibraryException*);
02275
02276
02277
02366 void SetAxisTargetVelocity(std::vector<int> const& axes, std::vector<double> const& velocities)
02367 throw (cSDHLibraryException*);
02368
02369
02370
02375 void SetAxisTargetVelocity(int iAxis, double velocity)
02376 throw (cSDHLibraryException*);
02377
02378
02379
02458 std::vector<double> SetAxisTargetGetAxisActualVelocity(std::vector<int> const& axes, std::vector<double> const& velocities)
02459 throw (cSDHLibraryException*);
02460
02461
02462
02511 std::vector<double> GetAxisTargetVelocity(std::vector<int> const& axes)
02512 throw (cSDHLibraryException*);
02513
02514
02515
02521 double GetAxisTargetVelocity(int iAxis)
02522 throw (cSDHLibraryException*);
02523
02524
02525
02574 std::vector<double> GetAxisLimitVelocity(std::vector<int> const& axes)
02575 throw (cSDHLibraryException*);
02576
02577
02578
02584 double GetAxisLimitVelocity(int iAxis)
02585 throw (cSDHLibraryException*);
02586
02587
02588
02637 std::vector<double> GetAxisLimitAcceleration(std::vector<int> const& axes)
02638 throw (cSDHLibraryException*);
02639
02640
02641
02647 double GetAxisLimitAcceleration(int iAxis)
02648 throw (cSDHLibraryException*);
02649
02650
02651
02696 std::vector<double> GetAxisActualVelocity(std::vector<int>const& axes)
02697 throw (cSDHLibraryException*);
02698
02699
02700
02706 double GetAxisActualVelocity(int iAxis)
02707 throw (cSDHLibraryException*);
02708
02709
02710
02767 std::vector<double> GetAxisReferenceVelocity(std::vector<int>const& axes)
02768 throw (cSDHLibraryException*);
02769
02770
02771
02777 double GetAxisReferenceVelocity(int iAxis)
02778 throw (cSDHLibraryException*);
02779
02780
02781
02863 void SetAxisTargetAcceleration(std::vector<int>const& axes, std::vector<double>const& accelerations)
02864 throw (cSDHLibraryException*);
02865
02866
02867
02872 void SetAxisTargetAcceleration(int iAxis, double acceleration)
02873 throw (cSDHLibraryException*);
02874
02875
02876
02925 std::vector<double> GetAxisTargetAcceleration(std::vector<int>const& axes)
02926 throw (cSDHLibraryException*);
02927
02928
02929
02935 double GetAxisTargetAcceleration(int iAxis)
02936 throw (cSDHLibraryException*);
02937
02938
02939
02994 std::vector<double> GetAxisMinAngle(std::vector<int> const& axes)
02995 throw (cSDHLibraryException*);
02996
02997
02998
03004 double GetAxisMinAngle(int iAxis)
03005 throw (cSDHLibraryException*);
03006
03007
03008
03063 std::vector<double> GetAxisMaxAngle(std::vector<int> const& axes)
03064 throw (cSDHLibraryException*);
03065
03066
03067
03073 double GetAxisMaxAngle(int iAxis)
03074 throw (cSDHLibraryException*);
03075
03076
03077
03137 std::vector<double> GetAxisMaxVelocity(std::vector<int> const& axes)
03138 throw (cSDHLibraryException*);
03139
03140
03141
03147 double GetAxisMaxVelocity(int iAxis)
03148 throw (cSDHLibraryException*);
03149
03150
03151
03206 std::vector<double> GetAxisMaxAcceleration(std::vector<int> const& axes)
03207 throw (cSDHLibraryException*);
03208
03209
03210
03216 double GetAxisMaxAcceleration(int iAxis)
03217 throw (cSDHLibraryException*);
03218
03219
03220
03331 double MoveAxis(std::vector<int>const& axes, bool sequ = true)
03332 throw (cSDHLibraryException*);
03333
03334
03335
03340 double MoveAxis(int iAxis, bool sequ = true)
03341 throw (cSDHLibraryException*);
03342
03343
03344
03345
03346
03347
03348
03349
03350
03352
03353
03354
03362
03424 void SetFingerEnable(std::vector<int> const& fingers, std::vector<double> const& states)
03425 throw (cSDHLibraryException*);
03426
03427
03428
03434 void SetFingerEnable(int iFinger, double state = 1.0)
03435 throw (cSDHLibraryException*);
03436
03437
03438
03444 void SetFingerEnable(std::vector<int> const& fingers, std::vector<bool> const& states)
03445 throw (cSDHLibraryException*);
03446
03447
03448
03454 void SetFingerEnable(int iFinger, bool state)
03455 throw (cSDHLibraryException*);
03456
03457
03458
03508 std::vector<double> GetFingerEnable(std::vector<int> const& fingers)
03509 throw (cSDHLibraryException*);
03510
03511
03512
03518 double GetFingerEnable(int iFinger)
03519 throw (cSDHLibraryException*);
03520
03521
03522
03582 void SetFingerTargetAngle(int iFinger, std::vector<double> const& angles)
03583 throw (cSDHLibraryException*);
03584
03585
03586
03591 void SetFingerTargetAngle(int iFinger, double a0, double a1, double a2)
03592 throw (cSDHLibraryException*);
03593
03594
03595
03634 std::vector<double> GetFingerTargetAngle(int iFinger)
03635 throw (cSDHLibraryException*);
03636
03637
03638
03643 void GetFingerTargetAngle(int iFinger, double& a0, double& a1, double& a2)
03644 throw (cSDHLibraryException*);
03645
03646
03647
03686 std::vector<double> GetFingerActualAngle(int iFinger)
03687 throw (cSDHLibraryException*);
03688
03689
03690
03695 void GetFingerActualAngle(int iFinger, double& a0, double& a1, double& a2)
03696 throw (cSDHLibraryException*);
03697
03698
03699
03747 std::vector<double> GetFingerMinAngle(int iFinger)
03748 throw (cSDHLibraryException*);
03749
03750
03751
03757 void GetFingerMinAngle(int iFinger, double& a0, double& a1, double& a2)
03758 throw (cSDHLibraryException*);
03759
03760
03761
03809 std::vector<double> GetFingerMaxAngle(int iFinger)
03810 throw (cSDHLibraryException*);
03811
03812
03813
03819 void GetFingerMaxAngle(int iFinger, double& a0, double& a1, double& a2)
03820 throw (cSDHLibraryException*);
03821
03822
03823
03824
03881 std::vector<double> GetFingerXYZ(int iFinger, std::vector<double> const& angles)
03882 throw (cSDHLibraryException*);
03883
03884
03885
03890 std::vector<double> GetFingerXYZ(int iFinger, double a0, double a1, double a2)
03891 throw (cSDHLibraryException*);
03892
03893
03894
03992 double MoveFinger(std::vector<int>const& fingers, bool sequ = true)
03993 throw (cSDHLibraryException*);
03994
03995
03996
04001 double MoveFinger(int iFinger, bool sequ = true)
04002 throw (cSDHLibraryException*);
04003
04004
04005
04023 double MoveHand(bool sequ = true)
04024 throw (cSDHLibraryException*);
04025
04026
04027
04029
04030
04031
04032
04040
04067 double GetGripMaxVelocity(void);
04068
04069
04070
04140 double GripHand(eGraspId grip, double close, double velocity, bool sequ = true)
04141 throw (cSDHLibraryException*);
04142
04143
04144
04146
04147
04148 private:
04152 void UpdateSettingsFromSDH();
04153
04160 void AdjustLimits(cSDHBase::eControllerType controller);
04161
04162
04164 std::string release_firmware;
04165
04167 eControllerType controller_type;
04168
04169
04170
04171
04172
04173
04174
04175
04176
04177
04178
04179
04180
04181
04182
04183 };
04184
04185
04186 NAMESPACE_SDH_END
04187
04188 #endif
04189
04190
04191
04192
04193
04194
04195
04196
04197
04198
04199
04200
04201