demo-benchmark.cpp
Go to the documentation of this file.
1 //======================================================================
40 //======================================================================
41 
42 #include <iostream>
43 #include <vector>
44 #define _USE_MATH_DEFINES
45 #include <math.h>
46 
47 // Include the cSDH interface
48 #include "sdh/sdh.h"
49 #include "sdh/util.h"
51 #include "sdh/basisdef.h"
52 #include "sdh/dsa.h"
53 #include "sdhoptions.h"
54 
56 
57 #define DEMO_BENCHMARK_USE_COMBINED_SET_GET 1
58 
65 char const* __help__ =
67  "Simple script to benchmark communication speed of the SDH:\n"
68  "The hand will move to a start position in coordinated position control\n"
69  "mode first. Then periodic movements are performed using the velocity\n"
70  "with acceleration ramp controller while the communication and control\n"
71  "rate is printed.\n"
72  "\n"
73  "- Example usage:\n"
74  " - Make SDH connected via Ethernet move.\n"
75  " The SDH has IP-Address 192.168.1.42 and is attached to TCP port 23.\n"
76  " (Requires at least SDH-firmware v0.0.3.1)\n"
77  " > demo-benchmark --tcp=192.168.1.42:23\n"
78  " \n"
79  " - Make SDH connected to port 2 = COM3 move:\n"
80  " > demo-benchmark -p 2 --dsaport=3\n"
81  " \n"
82  " - Make SDH connected to USB to RS232 converter 0 move:\n"
83  " > demo-benchmark --sdh_rs_device=/dev/ttyUSB0 --dsa_rs_device=/dev/ttyUSB1\n"
84  " \n"
85  " - Get the version info of both the joint controllers and the tactile \n"
86  " sensor firmware from an SDH connected via Ethernet.\n"
87  " The joint controllers and the tactile sensors have a common IP-Address,\n"
88  " here 192.168.1.42. The SDH controller is attached to the \n"
89  " default TCP port 23 and the tactile sensors to the default TCP port 13000.\n"
90  " (Requires at least SDH-firmware v0.0.3.2)\n"
91  " > demo-benchmark --tcp=192.168.1.42 --dsa_tcp -v\n"
92  " \n"
93  " - Get the version info of both the joint controllers and the tactile \n"
94  " sensor firmware from an SDH connected to:\n"
95  " - port 2 = COM3 (joint controllers) and \n"
96  " - port 3 = COM4 (tactile sensor controller) \n"
97  " > demo-benchmark --port=2 --dsaport=3 -v\n";
98 char const* __author__ = "Dirk Osswald: dirk.osswald@de.schunk.com";
99 char const* __url__ = "http://www.schunk.com";
100 char const* __version__ = "$Id: demo-benchmark.cpp 10895 2013-11-11 14:15:08Z Osswald2 $";
101 char const* __copyright__ = "Copyright (c) 2007 SCHUNK GmbH & Co. KG";
102 
103 // end of doxygen name group sdhlibrary_cpp_demo_benchmark_cpp_vars
104 // @}
105 
106 char const* usage =
107  "usage: demo-benchmark [options]\n"
108  ;
109 
110 cDBG cdbg( false, "red" );
111 
114 {
115  double t;
116  std::vector<double> aaa;
117  std::vector<double> aav;
118  std::vector<double> atv;
119 
120  sRecordedData( double _t, std::vector<double> const& _aaa, std::vector<double> const& _aav, std::vector<double> const& _atv ) :
121  t( _t ),
122  aaa( _aaa ),
123  aav( _aav ),
124  atv( _atv )
125  {};
126 };
127 //-----------------------------------------------------------------
128 
136 void GotoPose( cSDH& hand, std::vector<double>& ta )
137 {
138  hand.SetController( hand.eCT_POSE );
139  hand.SetAxisTargetVelocity( hand.All, 50 );
140 
141  hand.SetAxisTargetAngle( hand.all_real_axes, ta );
142  hand.MoveAxis( hand.All );
143 }
144 //-----------------------------------------------------------------
145 
146 
147 int main( int argc, char** argv )
148 {
150 
151  //---------------------
152  // handle command line options: set defaults first then overwrite by parsing actual command line
153  cSDHOptions options( SDHUSAGE_DEFAULT );
154 
155  options.Parse( argc, argv, __help__, "demo-benchmark", __version__, cSDH::GetLibraryName(), cSDH::GetLibraryRelease() );
156 
157  //
158  //---------------------
159 
160  //---------------------
161  // initialize debug message printing:
162  cdbg.SetFlag( options.debug_level > 0 );
163  cdbg.SetOutput( options.debuglog );
164 
165  g_sdh_debug_log = options.debuglog;
166 
167  cdbg << "Debug messages of " << argv[0] << " are printed like this.\n";
168 
169  // reduce debug level for subsystems
170  options.debug_level-=1;
171  //---------------------
172 
173  try
174  {
175  //###
176  // Create an instance "hand" of the class cSDH:
177  cdbg << "Connecting to joint controller...\n";
178  cSDH hand( false, false, options.debug_level );
179  cdbg << "Successfully created cSDH instance\n";
180 
181  // Open configured communication to the SDH device
182  options.OpenCommunication( hand );
183  cdbg << "Successfully opened communication to SDH\n";
184 
185  std::vector<double> max_vel = hand.GetAxisMaxVelocity( hand.all_real_axes );
186  unsigned int nb_axes = hand.all_real_axes.size();
187  //###
188 
189 
190  //###
191  // Prepare movements:
192 
193  // the given start positions:
194  std::vector<double> start_pose; // axis start positions in deg
195  start_pose.push_back( 10 );
196  start_pose.push_back( -10 );
197  start_pose.push_back( 0 );
198  start_pose.push_back( -10 );
199  start_pose.push_back( 0 );
200  start_pose.push_back( -10 );
201  start_pose.push_back( 0 );
202 
203  // the given end positions:
204  std::vector<double> end_pose; // axis end positions in deg
205  end_pose.push_back( 80.0 );
206  end_pose.push_back( -80.0 );
207  end_pose.push_back( -80.0 );
208  end_pose.push_back( -80.0 );
209  end_pose.push_back( -80.0 );
210  end_pose.push_back( -80.0 );
211  end_pose.push_back( -80.0 );
212 
213  // we want to make every axis move independently with velocity
214  // v(t)=a*sin(2*PI * t/p)
215  // for given period p and amplitude a
216  // the position is determined by:
217  // s(t)= a * p / (2*PI) * (1-cos(2*PI * t/p))
218  // The maximum position m is at p/2
219  // m= a*p/PI
220  // For fixed maximum position the amplitude thus calculates to
221  // a= m*PI/p
222 
223  // the given periods p:
224  std::vector<double> aperiod; // axis movement cycle periods in s
225  aperiod.push_back( 10.0 );
226  aperiod.push_back( 5.0 );
227  aperiod.push_back( 4.0 );
228  aperiod.push_back( 3.0 );
229  aperiod.push_back( 4.0 );
230  aperiod.push_back( 5.0 );
231  aperiod.push_back( 3.0 );
232 
233 
234  // the calculated amplidutes p of the sinusoidal
235  std::vector<double> aamplitude( aperiod.size(), 0.0 ); // axis movement velocity amplitude in deg/s
236  unsigned int ai;
237  for ( ai=0; ai<nb_axes; ai++ )
238  {
239  aamplitude[ai] = (end_pose[ai]-start_pose[ai]) * M_PI / aperiod[ai];
240  // limit to allowed range:
241  ToRange( aamplitude[ai], -max_vel[ai]+1.0, +max_vel[ai]-1.0 );
242  }
243  //###
244 
245 
246  //###
247  // move to start pose:
248  GotoPose( hand, start_pose );
249  //###
250 
251 
252  //###
253  // start moving sinusoidal:
254  cdbg << " Moving with velocity with acceleration ramp controller.\n";
255  cdbg.flush();
256 
257  // switch to velocity with acceleration ramp controller type:
259  hand.SetAxisTargetAcceleration( hand.All, 100 );
260 
261  std::vector<double> aaa(nb_axes, 0.0); // axis actual angles
262  std::vector<double> aav(nb_axes, 0.0); // axis actual velocities
263  std::vector<double> atv(nb_axes, 0.0); // axis target velocities
264 
265 
266  std::vector<double> min_angles = hand.GetAxisMinAngle( hand.all_real_axes );
267  std::vector<double> max_angles = hand.GetAxisMaxAngle( hand.all_real_axes );
268  std::vector<double> max_velocities = hand.GetAxisMaxVelocity( hand.all_real_axes );
269 
270 
271  std::vector<sRecordedData> recorded_data;
272 
273  //###
274  // perform motion for duration seconds
275  double t;
276  double const duration = 10.0; // duration in s
277  cSimpleTime start_time;
278  do
279  {
280  // get time elapsed since start_time
281  t = start_time.Elapsed();
282 
283  // Get current position of axes:
284  aaa = hand.GetAxisActualAngle( hand.all_real_axes );
285 
286  // calculate new target velocities for axes:
287  for ( ai=0; ai<nb_axes; ai++ )
288  {
289  atv[ai] = aamplitude[ai] * sin( 2.0*M_PI/aperiod[ai] * t );
290  }
291 
292 #if DEMO_BENCHMARK_USE_COMBINED_SET_GET
293  // Set target velocities and get current velocities of axes:
294  aav = hand.SetAxisTargetGetAxisActualVelocity( hand.all_real_axes, atv );
295 #else
296  // Get current velocities of axes:
297  aav = hand.GetAxisActualVelocity( hand.all_real_axes );
298 
299  hand.SetAxisTargetVelocity( hand.all_real_axes, atv );
300 #endif
301  //cdbg << t << ", " << aaa << ", " << aav << ", " << atv << "\n";
302  //cdbg << t << ", " << atv << "\n";
303  //std::cerr.flush();
304 
305  recorded_data.push_back( sRecordedData( t, aaa, aav, atv ) );
306  } while ( t < duration );
307 
308  cSimpleTime end_time;
309  //###
310 
311  //###
312  // stop motion softly within brake_time second
313  double brake_time = 0.5;
314  do {
315  t = end_time.Elapsed();
316 
317  for ( ai=0; ai<nb_axes; ai++ )
318  {
319  atv[ai] = aav[ai] * ( 1.0-t/brake_time);
320  }
321  hand.SetAxisTargetVelocity( hand.all_real_axes, atv );
322  } while ( t < 0.5 );
323  //###
324 
325 
326  //###
327  // open up again
328  GotoPose( hand, start_pose );
329  //
330  //###
331 
332 
333  //###
334  // Close the connection to the SDH
335  hand.Close();
336  cdbg << "Successfully disabled joint controllers of SDH and closed connection\n";
337  //###
338 
339  //###
340  // do evaluation
341  double dt_min = 1000.0;
342  double t_dt_min;
343  double dt_max = 0.0;
344  double t_dt_max;
345  double dt = 0.0;
346  int r = 0;
347  std::cout << "# combined gnuplot commands + data. Use plot.py for easy viewing\n";
348  std::cout << "## plot using 2:3 with lines title 'dt [s]'\n";
349  std::cout << "## plot using 2:4 with lines title 'aaa[0] [deg]'\n";
350  std::cout << "## plot using 2:5 with lines title 'aaa[1] [deg]'\n";
351  std::cout << "## plot using 2:6 with lines title 'aaa[2] [deg]'\n";
352  std::cout << "## plot using 2:7 with lines title 'aaa[3] [deg]'\n";
353  std::cout << "## plot using 2:8 with lines title 'aaa[4] [deg]'\n";
354  std::cout << "## plot using 2:9 with lines title 'aaa[5] [deg]'\n";
355  std::cout << "## plot using 2:10 with lines title 'aaa[6] [deg]'\n";
356  std::cout << "## plot using 2:11 with lines title 'aav[0] [deg/s]'\n";
357  std::cout << "## plot using 2:12 with lines title 'aav[1] [deg/s]'\n";
358  std::cout << "## plot using 2:13 with lines title 'aav[2] [deg/s]'\n";
359  std::cout << "## plot using 2:14 with lines title 'aav[3] [deg/s]'\n";
360  std::cout << "## plot using 2:15 with lines title 'aav[4] [deg/s]'\n";
361  std::cout << "## plot using 2:16 with lines title 'aav[5] [deg/s]'\n";
362  std::cout << "## plot using 2:17 with lines title 'aav[6] [deg/s]'\n";
363  std::cout << "## plot using 2:18 with lines title 'atv[0] [deg/s]'\n";
364  std::cout << "## plot using 2:19 with lines title 'atv[1] [deg/s]'\n";
365  std::cout << "## plot using 2:20 with lines title 'atv[2] [deg/s]'\n";
366  std::cout << "## plot using 2:21 with lines title 'atv[3] [deg/s]'\n";
367  std::cout << "## plot using 2:22 with lines title 'atv[4] [deg/s]'\n";
368  std::cout << "## plot using 2:23 with lines title 'atv[5] [deg/s]'\n";
369  std::cout << "## plot using 2:24 with lines title 'atv[6] [deg/s]'\n";
370  std::cout << "## set xlabel 'Time [s]'\n";
371  std::cout << "## set ylabel 'Control-Period / Position / Velocity [s] / [deg] / [deg/s]'\n";
372  std::cout << "## set grid\n";
373  std::cout << "## set title \"demo-benchmark: SDH moving in acceleration + velocity control mode\"\n";
374 
375  std::vector<sRecordedData>::iterator it0 = recorded_data.begin();
376  std::vector<sRecordedData>::iterator it1 = recorded_data.begin();
377  std::cout << "# i, t, dt, aaa[0..6], aav[0..6], atv[0..6]\n";
378  std::cout << r << ", " << it1->t << ", " << dt << ", " << it1->aaa << ", " << it1->aav << ", " << it1->atv << "\n";
379  for ( it1++; it1 < recorded_data.end(); it1++, it0++, r++ )
380  {
381  dt = it1->t - it0->t;
382  if ( dt < dt_min )
383  {
384  dt_min = dt;
385  t_dt_min = it1->t;
386  }
387  if ( dt > dt_max )
388  {
389  dt_max = dt;
390  t_dt_max = it1->t;
391  }
392 
393  std::cout << r << ", " << it1->t << ", " << dt << ", " << it1->aaa << ", " << it1->aav << ", " << it1->atv << "\n";
394  }
395  double dt_avg = start_time.Elapsed( end_time ) / recorded_data.size();
396  std::cout << "## set label 'dt_min=" << dt_min << "' at " << t_dt_min << "," << dt_min << " front point\n";
397  std::cout << "## set label 'dt_max=" << dt_max << "' at " << t_dt_max << "," << dt_max << " front point\n";
398  std::cout << "## set label 'dt_avg=" << dt_avg << "' at " << it0->t << "," << dt_avg << " front point\n";
399  std::cout << "## set title \"demo-benchmark.cpp: SDH moving in acceleration + velocity control mode\\ndt_avg = " << dt_avg << " fps = " << 1.0/dt_avg;
400  std::cout << "\\ndebug_level = " << options.debug_level+1 << " DEMO_BENCHMARK_USE_COMBINED_SET_GET = " << DEMO_BENCHMARK_USE_COMBINED_SET_GET << "\"\n";
401  std::cout.flush();
402  //###
403  }
404  catch (cSDHLibraryException* e)
405  {
406  std::cerr << "demo-benchmark main(): An exception was caught: " << e->what() << "\n";
407  delete e;
408  }
409 }
410 //----------------------------------------------------------------------
411 
412 
413 //======================================================================
414 /*
415  Here are some settings for the emacs/xemacs editor (and can be safely ignored)
416  (e.g. to explicitely set C++ mode for *.h header files)
417 
418  Local Variables:
419  mode:C++
420  mode:ELSE
421  End:
422 */
423 //======================================================================]
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
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...
char const * __help__
std::vector< double > GetAxisMaxVelocity(std::vector< int > const &axes)
Definition: sdh.cpp:1562
std::vector< double > aaa
#define SDH_ASSERT_TYPESIZES()
macro to assert that the defined typedefs have the expected sizes
Definition: basisdef.h:73
std::vector< double > aav
char const * __url__
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
void SetOutput(std::ostream *fd)
Definition: dbg.h:185
void SetFlag(bool flag)
Definition: dbg.h:149
velocity controller with acceleration ramp, velocities and accelerations of axes are controlled indep...
Definition: sdhbase.h:181
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
sRecordedData(double _t, std::vector< double > const &_aaa, std::vector< double > const &_aav, std::vector< double > const &_atv)
#define SDHUSAGE_DEFAULT
Definition: sdhoptions.h:64
std::vector< double > atv
coordinated position controller (position per axis => "pose controller"), all axes start and stop mov...
Definition: sdhbase.h:179
void SetAxisTargetAcceleration(std::vector< int >const &axes, std::vector< double >const &accelerations)
Definition: sdh.cpp:1457
char const * usage
void OpenCommunication(NS_SDH cSDH &hand)
Definition: sdhoptions.cpp:865
Base class for exceptions in the SDHLibrary-CPP.
Definition: sdhexception.h:132
std::ostream & flush()
Definition: dbg.h:289
void SetController(cSDHBase::eControllerType controller)
Definition: sdh.cpp:946
Interface of auxilliary utility functions for SDHLibrary-CPP.
char const * __author__
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
This file contains interface to #SDH::cDSA, a class to communicate with the tactile sensors of the SD...
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
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
void GotoPose(cSDH &hand, std::vector< double > &ta)
static char const * GetLibraryRelease(void)
Definition: sdh.cpp:626
structure to hold current hand state while recording with demo-benchmark
char const * __copyright__
double MoveAxis(std::vector< int >const &axes, bool sequ=true)
Definition: sdh.cpp:1898
int main(int argc, char **argv)
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 * __version__
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
This file contains some basic definitions (defines, macros, datatypes)
class for command line option parsing holding option parsing results
Definition: sdhoptions.h:78
std::vector< double > SetAxisTargetGetAxisActualVelocity(std::vector< int > const &axes, std::vector< double > const &velocities)
Definition: sdh.cpp:1318
std::vector< double > GetAxisActualVelocity(std::vector< int >const &axes)
Definition: sdh.cpp:1419
USING_NAMESPACE_SDH NAMESPACE_SDH_START std::ostream * g_sdh_debug_log
Definition: sdhbase.cpp:55
#define DEMO_BENCHMARK_USE_COMBINED_SET_GET
cDBG cdbg(false, "red")


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