demo-contact-grasping.cpp
Go to the documentation of this file.
1 //======================================================================
40 //======================================================================
41 
42 #include <iostream>
43 #include <vector>
44 
45 
46 // Include the cSDH interface
47 #include "sdh/sdh.h"
48 #include "sdh/util.h"
50 #include "sdh/basisdef.h"
51 #include "sdh/dsa.h"
52 #include "sdhoptions.h"
53 #include "dsaboost.h"
54 
56 
63 char const* __help__ =
65  "Simple script to do grasping with tactile sensor info feedback:\n"
66  "The hand will move to a pregrasp pose (open hand). You can then\n"
67  "reach an object to grasp into the hand. The actual grasping\n"
68  "is started as soon as a contact is detected. The finger\n"
69  "joints then try to move inwards until a certain\n"
70  "force is reached on the corresponding tactile sensors.\n"
71  "\n"
72  "- Example usage:\n"
73  " - Make SDH and tactile sensors connected via Ethernet grasp.\n"
74  " The SDH and the tactile sensors have a common IP-Address,\n"
75  " here 192.168.1.42. The SDH controller is attached to the \n"
76  " TCP port 23 and the tactile sensors to TCP port 13000.\n"
77  " (Requires at least SDH-firmware v0.0.3.2)\n"
78  " > demo-contact-grasping --tcp=192.168.1.42:23 --dsa_tcp=:13000\n"
79  " \n"
80  " - Make SDH connected to port 2 = COM3 with tactile sensors\n"
81  " connected to port 3 = COM4 grasp:\n"
82  " > demo-contact-grasping -p 2 --dsaport=3\n"
83  " \n"
84  " - Make SDH connected to USB to RS232 converter 0 and with tactile sensors\n"
85  " connected to USB to RS232 converter 1 grasp:\n"
86  " > demo-contact-grasping --sdh_rs_device=/dev/ttyUSB0 --dsa_rs_device=/dev/ttyUSB1\n"
87  " \n"
88  " - Get the version info of both the joint controllers and the tactile \n"
89  " sensor firmware from an SDH connected via Ethernet.\n"
90  " The joint controllers and the tactile sensors have a common IP-Address,\n"
91  " here 192.168.1.42. The SDH controller is attached to the \n"
92  " default TCP port 23 and the tactile sensors to the default TCP port 13000.\n"
93  " (Requires at least SDH-firmware v0.0.3.2)\n"
94  " > demo-contact-grasping --tcp=192.168.1.42 --dsa_tcp -v\n"
95  " \n"
96  " - Get the version info of both the joint controllers and the tactile \n"
97  " sensor firmware from an SDH connected to:\n"
98  " - port 2 = COM3 (joint controllers) and \n"
99  " - port 3 = COM4 (tactile sensor controller) \n"
100  " > demo-contact-grasping --port=2 --dsaport=3 -v\n";
101 char const* __author__ = "Dirk Osswald: dirk.osswald@de.schunk.com";
102 char const* __url__ = "http://www.schunk.com";
103 char const* __version__ = "$Id: demo-contact-grasping.cpp 10894 2013-11-11 13:58:38Z Osswald2 $";
104 char const* __copyright__ = "Copyright (c) 2007 SCHUNK GmbH & Co. KG";
105 
106 // end of doxygen name group sdhlibrary_cpp_demo_contact_grasping_cpp_vars
107 // @}
108 
109 char const* usage =
110  "usage: demo-contact-grasping [options]\n"
111  ;
112 
113 cDBG cdbg( false, "red" );
114 
115 //-----------------------------------------------------------------
116 
117 void GotoStartPose( cSDH& hand, char const* msg )
118 {
119  printf( "%s", msg );
120  hand.SetController( hand.eCT_POSE );
121  hand.SetAxisTargetVelocity( hand.All, 50 );
122  std::vector<double> fa;
123  fa.push_back( 90 );
124  fa.push_back( -45 );
125  fa.push_back( 0 );
126  fa.push_back( -90 );
127  fa.push_back( 0 );
128  fa.push_back( -45 );
129  fa.push_back( 0 );
130 
131  hand.SetAxisTargetAngle( hand.all_real_axes, fa );
132  hand.MoveAxis( hand.All );
133  printf( " OK\n" );
134  fflush( stdout );
135 }
136 //-----------------------------------------------------------------
137 
138 void AxisAnglesToFingerAngles( std::vector<double> aa, std::vector<double>& fa0, std::vector<double>& fa1, std::vector<double>& fa2 )
139 {
140  fa0[0] = aa[0];
141  fa0[1] = aa[1];
142  fa0[2] = aa[2];
143 
144  fa1[0] = 0.0;
145  fa1[1] = aa[3];
146  fa1[2] = aa[4];
147 
148  fa2[0] = aa[0];
149  fa2[1] = aa[5];
150  fa2[2] = aa[6];
151 }
152 //-----------------------------------------------------------------
153 
154 
155 int main( int argc, char** argv )
156 {
158 
159  //---------------------
160  // handle command line options: set defaults first then overwrite by parsing actual command line
161  cSDHOptions options( SDHUSAGE_DEFAULT " dsacom" );
162 
163 #if 0
164  int unused_opt_ind =
165 #endif
166  options.Parse( argc, argv, __help__, "demo-contact-grasping", __version__, cSDH::GetLibraryName(), cSDH::GetLibraryRelease() );
167 
168  //
169  //---------------------
170 
171  //---------------------
172  // initialize debug message printing:
173  cdbg.SetFlag( options.debug_level > 0 );
174  cdbg.SetOutput( options.debuglog );
175 
176  g_sdh_debug_log = options.debuglog;
177 
178  cdbg << "Debug messages of " << argv[0] << " are printed like this.\n";
179 
180  // reduce debug level for subsystems
181  options.debug_level-=1;
182  //---------------------
183 
184  cDSA* ts;
185  try
186  {
187  //###
188  // Create an instance "hand" of the class cSDH:
189  printf( "Connecting to joint controller..." );
190  cSDH hand( false, false, options.debug_level );
191  cdbg << "Successfully created cSDH instance\n";
192 
193  // Open configured communication to the SDH device
194  options.OpenCommunication( hand );
195  cdbg << "Successfully opened communication to SDH\n";
196 
197  printf( "OK\n" );
198  //###
199 
200 
201  //###
202  // Create a global instance at "ts" (tactile sensor) of the class cDSA according to the given options:
203 
204  // overwrite user given value
205  options.framerate = 30;
206  options.timeout = 1.0; // a real timeout is needed to make the closing of the connections work in case of errors / interrupt
207 
208  printf( "Connecting to tactile sensor controller. This may take up to 8 seconds..." );
209 
210  if ( options.dsa_use_tcp )
211  {
212  cdbg << "debug_level=" << options.debug_level << " tcp_adr=" << options.tcp_adr << " dsa_tcp_port=" << options.dsa_tcp_port << "\n";
213  ts = new cDSA( options.debug_level, options.tcp_adr.c_str(), options.dsa_tcp_port, options.timeout );
214  }
215  else
216  {
217  cdbg << "debug_level=" << options.debug_level << " dsaport=" << options.dsaport << " dsa_rs_device=" << options.dsa_rs_device << "\n";
218  ts = new cDSA( options.debug_level, options.dsaport, options.dsa_rs_device );
219  }
220  printf( "OK\n" );
221  //###
222 
223 
224  //###
225  // Prepare for grasping: open hand:
226  GotoStartPose( hand, "\nPreparing for grasping, opening hand (using POSE controller)..." );
227  //###
228 
229 
230  // Start reading tactile sensor info in a thread:
231  cDSAUpdater dsa_updater( ts, 8 );
232  boost::this_thread::sleep(boost::posix_time::milliseconds(200)); // give the updater a chance to read in first frame
233 
234  cDSA::sContactInfo contact_info;
235 
236  //###
237 #if 0
238  // for debugging, just output the local preprocessing results:
239  cIsGraspedByArea is_grasped_obj( ts );
240  double expected_area_ratio[6] = { 0.5, 0.5, 0.5, 0.5, 0.5, 0.5 };
241  for ( int i=0; i+unused_opt_ind<argc; i++ )
242  {
243  int rc = sscanf( argv[unused_opt_ind+i], "%lf", &expected_area_ratio[i] );
244  assert( rc == 1 );
245  }
246  is_grasped_obj.SetCondition( expected_area_ratio );
247 
248  while ( true )
249  {
250  for ( int m=0; m < 6; m++ )
251  {
252  contact_info = ts->GetContactInfo( m );
253  printf( " m=%d age=%ldms force=%6.1f x=%6.1f y=%6.1f area=%6.1f\n",
254  m,
256  contact_info.force, contact_info.cog_x, contact_info.cog_y, contact_info.area );
257  }
258  printf( "=> IsGrasped: %d\n", is_grasped_obj.IsGrasped() );
259 
260  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
261  }
262 #endif
263  //###
264 
265  //###
266  // wait for contact to start grasping
267  double desired_start_contact_area = 50.0;
268  double contact_area;
269  printf( "\nPress any tactile sensor on the SDH to start searching for contact..." ); fflush( stdout );
270  do
271  {
272  contact_area = 0.0;
273 
274  for ( int m=0; m < 6; m++ )
275  {
276  contact_area += ts->GetContactArea( m );
277  //printf( " m=%d area2=%6.1f\n", m, ts->GetContactArea( m ) );
278  }
279 
280  cdbg << " contact area too small\n";
281  boost::this_thread::sleep(boost::posix_time::milliseconds(200));
282  } while ( contact_area < desired_start_contact_area );
283 
284  // wait until that contact is released on the middle finger
285  while ( ts->GetContactArea( 2 ) + ts->GetContactArea( 3 ) > desired_start_contact_area )
286  {
287  boost::this_thread::sleep(boost::posix_time::milliseconds(200)); // give the updater a chance to read in first frame
288  }
289 
290  printf( " OK, contact detected: %f mm*mm\n", contact_area ); fflush( stdout );
291  //###
292 
293  //###
294  // start grasping
295  double desired_force = 5.0; //options.desired_force // the desired force that every sensor patch should reach
296  double vel_per_force = 2.5; // factor for converting force to velocity
297  double loop_time = 0.01;
298  int loop_time_ms = (int) (loop_time * 1000.0);
299 
300  bool finished = false;
301  printf( "Simple force based grasping starts:\n" );
302  printf( " Joints of the opposing fingers 1 and 3 (axes 1,2,5,6) will move in\n" );
303  printf( " positive direction (inwards) as long as the contact force measured\n" );
304  printf( " by the tactile sensor patch of that joint is less than\n" );
305  printf( " the desired force %f\n", desired_force );
306  printf( " This will use the velocity with acceleration ramp controller.\n" );
307  printf( "\n" );
308  printf( " Press a tactile sensor on the middle finger 2 to stop!\n" );
309  fflush( stdout );
310 
311 
312  // switch to velocity with acceleration ramp controller type:
314  hand.SetAxisTargetAcceleration( hand.All, 100 );
315  cdbg << "max=" << hand.GetAxisMaxVelocity( hand.all_real_axes ) << "\n";
316 
317  std::vector<double> aaa; // axis actual angles
318  std::vector<double> ata; // axis target angles
319  std::vector<double> atv; // axist target velocities
320  std::vector<double> fta0(3); // finger target angles
321  std::vector<double> fta1(3); // finger target angles
322  std::vector<double> fta2(3); // finger target angles
323  std::vector<double> fta[3];
324  fta[0] = fta0;
325  fta[1] = fta1;
326  fta[2] = fta2;
327 
328  std::vector<double> min_angles = hand.GetAxisMinAngle( hand.all_real_axes );
329  std::vector<double> max_angles = hand.GetAxisMaxAngle( hand.all_real_axes );
330  std::vector<double> max_velocities = hand.GetAxisMaxVelocity( hand.all_real_axes );
331 
332  atv = hand.GetAxisTargetVelocity( hand.all_real_axes );
333  while (true)
334  {
335  int nb_ok = 0;
336 
337  //###
338  // check for stop condition (contact on middle finger)
339  cDSA::sContactInfo contact_middle_prox = ts->GetContactInfo( 2 );
340  cDSA::sContactInfo contact_middle_dist = ts->GetContactInfo( 3 );
341  if ( contact_middle_prox.area + contact_middle_dist.area > desired_start_contact_area )
342  {
343  printf( "\nContact on middle finger detected! Stopping demonstration.\n" ); fflush( stdout );
344  finished = true;
345  break;
346  }
347  //
348  //###
349 
350 
351  // Get current position of axes:
352  // (Limited to the allowed range. This is necessary since near the
353  // range limits an axis might report an angle slightly off range.)
354  std::vector<double> aaa = hand.GetAxisActualAngle( hand.all_real_axes );
355  ToRange( aaa, min_angles, max_angles );
356  ata = aaa;
357 
358  // for the selected axes:
359  int ais[] = { 1,2, 5,6 }; // indices of used motor axes
360  int mis[] = { 0,1, 4,5 }; // indices of used tactile sensors
361  for ( int i=0; i < (int) (sizeof( ais ) / sizeof( int )); i++ )
362  {
363  int ai = ais[i];
364  int mi = mis[i];
365  // read the contact force of the tactile sensor of the axis
366  contact_info = ts->GetContactInfo( mi );
367  //cdbg << "%d,%d,%d force=%6.1f x=%6.1f y=%6.1f area=%6.1f\n" % (ai, fi, part, force, cog_x, cog_y, area) # pylint: disable-msg=W0104
368  cdbg << mi << " force=" << contact_info.force << "\n";
369 
370  // translate the measured force into a new velocity for the axis
371  // in order to reach the desired_force
372  atv[ai] = (desired_force - contact_info.force) * vel_per_force;
373 
374  // limit the calculated target velocities to the allowed range:
375  atv[ai] = ToRange( atv[ai], -max_velocities[ai], max_velocities[ai] );
376 
377  if ( contact_info.force < desired_force )
378  {
379  cdbg << "closing axis " << ai << " with " << atv[ai] << " deg/s\n";
380  }
381  else if ( contact_info.force > desired_force )
382  {
383  cdbg << "opening axis " << ai << " with " << atv[ai] << " deg/s\n";
384  }
385  else
386  {
387  cdbg << "axis " << ai << " ok\n";
388  nb_ok += 1;
389 
390  }
391 
392  //###
393  // check if the fingers would get closer than 10mm with the calculated position:
394  // calculate future position roughly according to determined velocity:
395  ata[ai] += atv[ai] * loop_time; // s = v * t => s(t') = s(t) + v * dt
396 
397 
398  AxisAnglesToFingerAngles( ata, fta[0], fta[1], fta[2] );
399 
400  // TODO: CheckFingerCollisions not implemented in SDHLibrary-C++
401 #if 0
402  (cxy, (c01,d01), (c02,d02), (c12,d12)) = hand.CheckFingerCollisions( fta[0], fta[1], fta[2] );
403  d_min = min( d01, d02, d12);
404  if ( (cxy || d_min < 2.0) && force < desired_force )
405  {
406  // if there would be a collision then do not move the current axis there
407  printf( "not moving axis %d further due to internal collision (d_min=%f)\n", ai, d_min );
408  ata[ai] = aaa[ai];
409  atv[ai] = 0.0;
410  }
411 #else
412  // simple approach for now: dont let any finger move into the other fingers half
413  int fi = ( (ai<=2) ? 0 : 2 );
414  std::vector<double> xyz = hand.GetFingerXYZ( fi, fta[fi] );
415 
416  switch (fi)
417  {
418  case 0:
419  if ( xyz[0] <= 6.0 )
420  {
421  // if there would be a collision then do not move the current axis there
422  printf( "not moving axis %d further due to quadrant crossing of finger %d xyz= %6.1f,%6.1f,%6.1f\n", ai, fi, xyz[0],xyz[1],xyz[2] );
423  ata[ai] = aaa[ai];
424  atv[ai] = 0.0;
425  }
426  break;
427  case 2:
428  if ( xyz[0] >= -6.0 )
429  {
430  // if there would be a collision then do not move the current axis there
431  printf( "not moving axis %d further due to quadrant crossing of finger %d xyz= %6.1f,%6.1f,%6.1f\n", ai, fi, xyz[0],xyz[1],xyz[2] );
432  ata[ai] = aaa[ai];
433  atv[ai] = 0.0;
434  }
435  break;
436  default:
437  assert( fi == 0 || fi == 2 );
438  break;
439  }
440 #endif
441  //
442  //###
443 
444  } //
445  // a new target velocity has been calculated from the tactile sensor data, so move accordingly
446  cdbg.PDM( "moving with %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f deg/s\n", atv[0], atv[1], atv[2], atv[3], atv[4], atv[5], atv[6] );
447  hand.SetAxisTargetVelocity( hand.all_real_axes, atv );
448  finished = (nb_ok == 6);
449 
450  fflush( stdout );
451  boost::this_thread::sleep(boost::posix_time::milliseconds(loop_time_ms));
452  }
453  cdbg << "after endless loop\n";
454 
455 
456 
457  //###
458  // open up again
459  GotoStartPose( hand, "\nReopening hand (using POSE controller)..." );
460  //
461  //###
462 
463 
464  //###
465  // stop sensor:
466  dsa_updater.interrupt();
467 
468  ts->Close();
469  cdbg << "Successfully disabled tactile sensor controller of SDH and closed connection\n";
470 
471  // Close the connection to the SDH and DSA
472  hand.Close();
473  cdbg << "Successfully disabled joint controllers of SDH and closed connection\n";
474 
475 
476  }
477  catch (cSDHLibraryException* e)
478  {
479  std::cerr << "demo-contact-grasping main(): An exception was caught: " << e->what() << "\n";
480  delete e;
481  }
482 }
483 //----------------------------------------------------------------------
484 
485 
486 //======================================================================
487 /*
488  Here are some settings for the emacs/xemacs editor (and can be safely ignored)
489  (e.g. to explicitely set C++ mode for *.h header files)
490 
491  Local Variables:
492  mode:C++
493  mode:ELSE
494  End:
495 */
496 //======================================================================]
class for calculation of IsGrasped condition using an expected area of contact measured by the tactil...
Definition: dsaboost.h:122
Structure to hold info about the contact of one sensor patch.
Definition: dsa.h:249
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
helper stuff for the DSA using boost
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
char const * usage
std::string tcp_adr
Definition: sdhoptions.h:124
This file contains the interface to class #SDH::cSDH, the end user class to access the SDH from a PC...
sTactileSensorFrame const & GetFrame() const
return a reference to the latest tactile sensor frame without reading from remote DSACON32m ...
Definition: dsa.h:493
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
#define SDH_ASSERT_TYPESIZES()
macro to assert that the defined typedefs have the expected sizes
Definition: basisdef.h:73
int main(int argc, char **argv)
A class to print colored debug messages.
Definition: dbg.h:113
void SetOutput(std::ostream *fd)
Definition: dbg.h:185
char const * __copyright__
void SetFlag(bool flag)
Definition: dbg.h:149
char const * __url__
char const * __author__
velocity controller with acceleration ramp, velocities and accelerations of axes are controlled indep...
Definition: sdhbase.h:181
bool dsa_use_tcp
Definition: sdhoptions.h:127
char const * __version__
virtual const char * what() const
std::ostream * debuglog
Definition: sdhoptions.h:86
void Close(void)
Set the framerate of the remote DSACON32m controller to 0 and close connection to it...
Definition: dsa.cpp:739
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
double GetContactArea(int m)
Definition: dsa.cpp:982
std::vector< double > GetAxisTargetVelocity(std::vector< int > const &axes)
Definition: sdh.cpp:1329
#define SDHUSAGE_DEFAULT
Definition: sdhoptions.h:64
double force
accumulated force in N
Definition: dsa.h:251
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
int dsa_tcp_port
Definition: sdhoptions.h:128
void OpenCommunication(NS_SDH cSDH &hand)
Definition: sdhoptions.cpp:865
SDH::cDSA is the end user interface class to access the DSACON32m, the tactile sensor controller of t...
Definition: dsa.h:111
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
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
cDBG cdbg(false, "red")
void SetCondition(double eaf0p, double eaf0d, double eaf1p, double eaf1d, double eaf2p, double eaf2d)
Definition: dsaboost.cpp:172
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
sContactInfo GetContactInfo(int m)
Definition: dsa.cpp:1026
static char const * GetLibraryRelease(void)
Definition: sdh.cpp:626
double MoveAxis(std::vector< int >const &axes, bool sequ=true)
Definition: sdh.cpp:1898
char const * __help__
This file contains settings to make the SDHLibrary compile on differen systems:
char dsa_rs_device[MAX_DEV_LENGTH]
Definition: sdhoptions.h:108
A data structure describing a full tactile sensor frame read from the remote DSACON32m controller...
Definition: dsa.h:235
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
Class to create an updater thread for continuously updating tactile sensor data.
Definition: dsaboost.h:61
virtual bool IsGrasped(void)
Definition: dsaboost.cpp:202
double cog_y
center of gravity of contact in y direction of sensor patch in mm
Definition: dsa.h:254
void interrupt()
interrupt the updater thread
Definition: dsaboost.h:93
USING_NAMESPACE_SDH NAMESPACE_SDH_START std::ostream * g_sdh_debug_log
Definition: sdhbase.cpp:55
double area
area of contact in mm*mm.
Definition: dsa.h:252
void AxisAnglesToFingerAngles(std::vector< double > aa, std::vector< double > &fa0, std::vector< double > &fa1, std::vector< double > &fa2)
void GotoStartPose(cSDH &hand, char const *msg)
void PDM(char const *fmt,...) SDH__attribute__((format(printf
unsigned long GetAgeOfFrame(sTactileSensorFrame *frame_p)
Definition: dsa.h:651
double cog_x
center of gravity of contact in x direction of sensor patch in mm
Definition: dsa.h:253
double timeout
Definition: sdhoptions.h:90


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