demo-mimic.cpp
Go to the documentation of this file.
1 //======================================================================
37 //======================================================================
38 
39 //----------------------------------------------------------------------
40 // System Includes - include with <>
41 //----------------------------------------------------------------------
42 
43 #include <getopt.h>
44 #include <assert.h>
45 
46 #include <iostream>
47 #include <vector>
48 
49 using namespace std;
50 
51 //----------------------------------------------------------------------
52 // Project Includes - include with ""
53 //---------------------------------------------------------------------
54 
55 #include "sdh/sdh.h"
56 #include "sdh/simpletime.h"
57 #include "sdh/util.h"
59 #include "sdh/basisdef.h"
60 #include "sdhoptions.h"
61 
62 //----------------------------------------------------------------------
63 // Defines, enums, unions, structs,
64 //----------------------------------------------------------------------
65 
67 
68 
69 //----------------------------------------------------------------------
70 // Global variables
71 //----------------------------------------------------------------------
72 
79 char const* __help__ =
81  "In case you have 2 SDHs you can operate one of them by moving the first\n"
82  "hand manualy. You must give parameters for 2 hands on the command line.\n"
83  "(C++ demo application using the SDHLibrary-CPP library.)\n"
84  "\n"
85  "- Example usage:\n"
86  " - Mimic the manual movements of SDH on port 2 = COM3 \n"
87  " with the SDH on port 5 = COM6:\n"
88  " > demo-mimic -p 2 -p 5\n"
89  " \n"
90  " - Mimic the manual movements of SDH with CAN IDs 0x01 and 0x11 on\n"
91  " ESD CAN bus with the SDH with CAN IDs 0x02 and 0x2 on the same\n"
92  " ESD CAN bus\n"
93  " > demo-mimic --can --id_read 0x1 --id_write 0x11 --can --id_read 0x2 --id_write 0x22\n"
94  " \n";
95 char const* __author__ = "Dirk Osswald: dirk.osswald@de.schunk.com";
96 char const* __url__ = "http://www.schunk.com";
97 char const* __version__ = "$Id: demo-mimic.cpp 6501 2011-03-01 17:35:45Z Osswald2 $";
98 char const* __copyright__ = "Copyright (c) 2007 SCHUNK GmbH & Co. KG";
99 
100 // end of doxygen name group sdhlibrary_cpp_demo_mimic_cpp_vars
101 // @}
102 
103 //----------------------------------------------------------------------
104 // Function and class member implementation (function definitions)
105 //----------------------------------------------------------------------
106 
107 // create a cSDH object according to \a options and return a ptr to it
108 cSDH* GetHand( cSDHOptions& options, cDBG& cdbg, int nb )
109 {
110  // cSDH instance "hand" of the class cSDH according to the given options:
111  cSDH* hand = new cSDH( options.use_radians, options.use_fahrenheit, options.debug_level );
112  cdbg << "Successfully created cSDH instance " << nb << "\n";
113 
114  // Open configured communication to the SDH device
115  options.OpenCommunication( *hand );
116  cdbg << "Successfully opened communication to SDH " << nb << "\n";
117 
118  return hand;
119 }
120 
121 
122 int main( int argc, char **argv )
123 {
125 
126  //---------------------
127  // this script is special since it uses 2 SDHs,
128 
129  // handle command line options: set defaults first then overwrite by parsing actual command line
130  cSDHOptions options1( SDHUSAGE_DEFAULT " sdhother" );
131  cSDHOptions options2( SDHUSAGE_DEFAULT " sdhother" );
132 
133  options1.period = 0.5;
134 
135  // so detect where the options for the 2nd hand begin
136  int i = 0;
137  int nb_sdh = 0;
138  while ( i<argc && nb_sdh < 2 )
139  {
140  if ( !strncmp( argv[i], "-p", strlen( "-p" ) ) ||
141  !strncmp( argv[i], "--port", strlen( "--port" ) ) ||
142  !strncmp( argv[i], "--sdhport", strlen( "--sdhport" ) ) ||
143  !strncmp( argv[i], "--sdh_rs_device", strlen( "--sdh_rs_device" ) ) ||
144  !strncmp( argv[i], "-c", strlen( "-c" ) ) ||
145  !strncmp( argv[i], "--can", strlen( "--can" ) ) ||
146  !strncmp( argv[i], "--canesd", strlen( "--canesd" ) ) ||
147  !strncmp( argv[i], "--canpeak", strlen( "--canpeak" ) ) ||
148  !strncmp( argv[i], "--sdh_canpeak_device", strlen( "--sdh_canpeak_device" ) ) )
149  nb_sdh++;
150  i++;
151  }
152  // parse options for 1st hand:
153  options1.Parse( i-((nb_sdh==2)*1), argv, __help__, "demo-mimic", __version__, cSDH::GetLibraryName(), cSDH::GetLibraryRelease() );
154 
155  if ( nb_sdh != 2 )
156  {
157  cerr << "You must provide parameters for exactly 2 SDHs. See --help\n";
158  exit(1);
159  }
160 
161  // parse options for 2nd hand:
162  // add fake argv[0]
163  argv[i-2] = argv[0];
164  optind = 1;
165  options2.Parse( argc-i+2, argv+i-2, __help__, "demo-mimic", __version__, cSDH::GetLibraryName(), cSDH::GetLibraryRelease() );
166  //
167  //---------------------
168 
169  //---------------------
170  // initialize debug message printing:
171  if ( options2.debuglog != &cerr)
172  g_sdh_debug_log = options2.debuglog;
173  else
174  g_sdh_debug_log = options1.debuglog;
175 
176  cDBG cdbg( (options1.debug_level + options2.debug_level) > 0, "red", g_sdh_debug_log );
177 
178  cdbg << "Debug messages of " << argv[0] << " are printed like this.\n";
179 
180  // reduce debug level for subsystems
181  options1.debug_level-=1;
182  options2.debug_level-=1;
183  //---------------------
184 
185  try
186  {
187  cSDH* hand[2];
188 
189  hand[0] = GetHand( options1, cdbg, 0 );
190  hand[1] = GetHand( options2, cdbg, 1 );
191 
192 
193  hand[0]->SetAxisEnable( cSDH::All, false );
194 
195  hand[1]->SetController( cSDH::eCT_POSE );
197  hand[1]->SetAxisTargetVelocity( cSDH::All, 40.0 );
198  hand[1]->SetAxisEnable( cSDH::All, true );
199 
200  vector<double> min_angles = hand[1]->GetAxisMinAngle( hand[1]->all_real_axes );
201  vector<double> max_angles = hand[1]->GetAxisMaxAngle( hand[1]->all_real_axes );
202  vector<double> angles;
203 
204 #if SDH_USE_VCC
205  double elapsed = 0.0;
206 #else
207  cSimpleTime start;
208 #endif
209 
210  angles = hand[0]->GetAxisActualAngle( hand[0]->all_real_axes );
211  ToRange( angles, min_angles, max_angles );
212  hand[1]->SetAxisEnable( cSDH::All, true );
213  hand[1]->MoveHand(true);
214 
215  while (true)
216  {
217  angles = hand[0]->GetAxisActualAngle( hand[0]->all_real_axes );
218  ToRange( angles, min_angles, max_angles );
219  hand[1]->SetAxisTargetAngle( hand[1]->all_real_axes, angles );
220  hand[1]->MoveHand(false);
221 
222 #if SDH_USE_VCC
223  cout << elapsed << " ";
224  elapsed += options1.period;
225 #else
226  cout << start.Elapsed() << " ";
227 #endif
228 
229  for ( vector<double>::const_iterator ai = angles.begin();
230  ai != angles.end();
231  ai++ )
232  cout << *ai << " ";
233 
234  cout << "\n";
235  cout.flush();
236 
237  SleepSec( options1.period );
238  }
239 
240  hand[0]->Close();
241  hand[1]->Close();
242  cdbg << "Successfully disabled controllers of SDHs and closed connections\n";
243  }
244  catch ( cSDHLibraryException* e )
245  {
246  cerr << "demo-mimic main(): Caught exception from SDHLibrary: " << e->what() << ". Giving up!\n";
247  delete e;
248  }
249  catch (...)
250  {
251  cerr << "caught unexpected exception!\n";
252  }
253 }
254 //----------------------------------------------------------------------
255 
256 
257 //======================================================================
258 /*
259  Here are some settings for the emacs/xemacs editor (and can be safely ignored):
260  (e.g. to explicitely set C++ mode for *.h header files)
261 
262  Local Variables:
263  mode:C++
264  mode:ELSE
265  End:
266 */
267 //======================================================================
A meta-value that means "access all possible values".
Definition: sdhbase.h:103
#SDH::cSDH is the end user interface class to control a SDH (SCHUNK Dexterous Hand).
Definition: sdh.h:172
char const * __version__
Definition: demo-mimic.cpp:97
int Parse(int argc, char **argv, char const *helptext, char const *progname, char const *version, char const *libname, char const *librelease)
Definition: sdhoptions.cpp:464
This file contains the interface to class #SDH::cSDH, the end user class to access the SDH from a PC...
bool use_fahrenheit
Definition: sdhoptions.h:104
#define SDH_ASSERT_TYPESIZES()
macro to assert that the defined typedefs have the expected sizes
Definition: basisdef.h:73
int optind
Definition: getopt.c:131
Interface of auxilliary utility functions for SDHLibrary-CPP.
void SetVelocityProfile(eVelocityProfile velocity_profile)
Definition: sdh.cpp:986
int main(int argc, char **argv)
Definition: demo-mimic.cpp:122
double Elapsed(void) const
Return time in seconds elapsed between the time stored in the object and now.
Definition: simpletime.h:115
A class to print colored debug messages.
Definition: dbg.h:113
ramp velocity profile
Definition: sdhbase.h:195
bool use_radians
Definition: sdhoptions.h:103
void SetAxisEnable(std::vector< int > const &axes, std::vector< double > const &states)
Definition: sdh.cpp:1055
virtual const char * what() const
std::ostream * debuglog
Definition: sdhoptions.h:86
int debug_level
Definition: sdhoptions.h:85
Implementation of a class to parse common SDH related command line options.
static char const * GetLibraryName(void)
Definition: sdh.cpp:633
#define SDHUSAGE_DEFAULT
Definition: sdhoptions.h:64
void SleepSec(double t)
Definition: util.cpp:155
double period
Definition: sdhoptions.h:105
coordinated position controller (position per axis => "pose controller"), all axes start and stop mov...
Definition: sdhbase.h:179
char const * __copyright__
Definition: demo-mimic.cpp:98
char const * __author__
Definition: demo-mimic.cpp:95
void OpenCommunication(NS_SDH cSDH &hand)
Definition: sdhoptions.cpp:865
Base class for exceptions in the SDHLibrary-CPP.
Definition: sdhexception.h:132
void SetController(cSDHBase::eControllerType controller)
Definition: sdh.cpp:946
Interface of auxilliary utility functions for SDHLibrary-CPP.
void Close(bool leave_enabled=false)
Definition: sdh.cpp:888
void SetAxisTargetVelocity(std::vector< int > const &axes, std::vector< double > const &velocities)
Definition: sdh.cpp:1289
void SetAxisTargetAngle(std::vector< int > const &axes, std::vector< double > const &angles)
Definition: sdh.cpp:1209
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
#define USING_NAMESPACE_SDH
char const * __help__
Definition: demo-mimic.cpp:80
static char const * GetLibraryRelease(void)
Definition: sdh.cpp:626
This file contains settings to make the SDHLibrary compile on differen systems:
Very simple class to measure elapsed time.
Definition: simpletime.h:84
char const * __url__
Definition: demo-mimic.cpp:96
cSDH * GetHand(cSDHOptions &options, cDBG &cdbg, int nb)
Definition: demo-mimic.cpp:108
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
double MoveHand(bool sequ=true)
Definition: sdh.cpp:2039
This file contains some basic definitions (defines, macros, datatypes)
class for command line option parsing holding option parsing results
Definition: sdhoptions.h:78
USING_NAMESPACE_SDH NAMESPACE_SDH_START std::ostream * g_sdh_debug_log
Definition: sdhbase.cpp:55
cDBG cdbg(false, "red")


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