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" 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" 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" 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" 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" 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 $";
110 "usage: demo-contact-grasping [options]\n" 122 std::vector<double> fa;
138 void AxisAnglesToFingerAngles( std::vector<double> aa, std::vector<double>& fa0, std::vector<double>& fa1, std::vector<double>& fa2 )
155 int main(
int argc,
char** argv )
178 cdbg <<
"Debug messages of " << argv[0] <<
" are printed like this.\n";
189 printf(
"Connecting to joint controller..." );
191 cdbg <<
"Successfully created cSDH instance\n";
195 cdbg <<
"Successfully opened communication to SDH\n";
208 printf(
"Connecting to tactile sensor controller. This may take up to 8 seconds..." );
226 GotoStartPose( hand,
"\nPreparing for grasping, opening hand (using POSE controller)..." );
232 boost::this_thread::sleep(boost::posix_time::milliseconds(200));
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++ )
243 int rc = sscanf( argv[unused_opt_ind+i],
"%lf", &expected_area_ratio[i] );
250 for (
int m=0; m < 6; m++ )
253 printf(
" m=%d age=%ldms force=%6.1f x=%6.1f y=%6.1f area=%6.1f\n",
258 printf(
"=> IsGrasped: %d\n", is_grasped_obj.
IsGrasped() );
260 boost::this_thread::sleep(boost::posix_time::milliseconds(100));
267 double desired_start_contact_area = 50.0;
269 printf(
"\nPress any tactile sensor on the SDH to start searching for contact..." ); fflush( stdout );
274 for (
int m=0; m < 6; m++ )
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 );
287 boost::this_thread::sleep(boost::posix_time::milliseconds(200));
290 printf(
" OK, contact detected: %f mm*mm\n", contact_area ); fflush( stdout );
295 double desired_force = 5.0;
296 double vel_per_force = 2.5;
297 double loop_time = 0.01;
298 int loop_time_ms = (int) (loop_time * 1000.0);
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" );
308 printf(
" Press a tactile sensor on the middle finger 2 to stop!\n" );
317 std::vector<double> aaa;
318 std::vector<double> ata;
319 std::vector<double> atv;
320 std::vector<double> fta0(3);
321 std::vector<double> fta1(3);
322 std::vector<double> fta2(3);
323 std::vector<double> fta[3];
341 if ( contact_middle_prox.
area + contact_middle_dist.
area > desired_start_contact_area )
343 printf(
"\nContact on middle finger detected! Stopping demonstration.\n" ); fflush( stdout );
355 ToRange( aaa, min_angles, max_angles );
359 int ais[] = { 1,2, 5,6 };
360 int mis[] = { 0,1, 4,5 };
361 for (
int i=0; i < (int) (
sizeof( ais ) /
sizeof(
int )); i++ )
368 cdbg << mi <<
" force=" << contact_info.
force <<
"\n";
372 atv[ai] = (desired_force - contact_info.
force) * vel_per_force;
375 atv[ai] =
ToRange( atv[ai], -max_velocities[ai], max_velocities[ai] );
377 if ( contact_info.
force < desired_force )
379 cdbg <<
"closing axis " << ai <<
" with " << atv[ai] <<
" deg/s\n";
381 else if ( contact_info.
force > desired_force )
383 cdbg <<
"opening axis " << ai <<
" with " << atv[ai] <<
" deg/s\n";
387 cdbg <<
"axis " << ai <<
" ok\n";
395 ata[ai] += atv[ai] * loop_time;
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 )
407 printf(
"not moving axis %d further due to internal collision (d_min=%f)\n", ai, d_min );
413 int fi = ( (ai<=2) ? 0 : 2 );
414 std::vector<double> xyz = hand.
GetFingerXYZ( fi, fta[fi] );
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] );
428 if ( xyz[0] >= -6.0 )
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] );
437 assert( fi == 0 || fi == 2 );
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] );
448 finished = (nb_ok == 6);
451 boost::this_thread::sleep(boost::posix_time::milliseconds(loop_time_ms));
453 cdbg <<
"after endless loop\n";
459 GotoStartPose( hand,
"\nReopening hand (using POSE controller)..." );
469 cdbg <<
"Successfully disabled tactile sensor controller of SDH and closed connection\n";
473 cdbg <<
"Successfully disabled joint controllers of SDH and closed connection\n";
479 std::cerr <<
"demo-contact-grasping main(): An exception was caught: " << e->
what() <<
"\n";
class for calculation of IsGrasped condition using an expected area of contact measured by the tactil...
A meta-value that means "access all possible values".
#SDH::cSDH is the end user interface class to control a SDH (SCHUNK Dexterous Hand).
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)
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 ...
std::vector< double > GetAxisMaxVelocity(std::vector< int > const &axes)
std::vector< double > GetFingerXYZ(int iFinger, std::vector< double > const &angles)
#define SDH_ASSERT_TYPESIZES()
macro to assert that the defined typedefs have the expected sizes
A class to print colored debug messages.
void SetOutput(std::ostream *fd)
velocity controller with acceleration ramp, velocities and accelerations of axes are controlled indep...
virtual const char * what() const
void Close(void)
Set the framerate of the remote DSACON32m controller to 0 and close connection to it...
Implementation of a class to parse common SDH related command line options.
static char const * GetLibraryName(void)
double GetContactArea(int m)
std::vector< double > GetAxisTargetVelocity(std::vector< int > const &axes)
coordinated position controller (position per axis => "pose controller"), all axes start and stop mov...
void SetAxisTargetAcceleration(std::vector< int >const &axes, std::vector< double >const &accelerations)
void OpenCommunication(NS_SDH cSDH &hand)
SDH::cDSA is the end user interface class to access the DSACON32m, the tactile sensor controller of t...
Base class for exceptions in the SDHLibrary-CPP.
void SetController(cSDHBase::eControllerType controller)
Interface of auxilliary utility functions for SDHLibrary-CPP.
void Close(bool leave_enabled=false)
void SetAxisTargetVelocity(std::vector< int > const &axes, std::vector< double > const &velocities)
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.
void SetAxisTargetAngle(std::vector< int > const &axes, std::vector< double > const &angles)
void SetCondition(double eaf0p, double eaf0d, double eaf1p, double eaf1d, double eaf2p, double eaf2d)
double ToRange(double v, double min, double max)
std::vector< double > GetAxisMinAngle(std::vector< int > const &axes)
#define USING_NAMESPACE_SDH
sContactInfo GetContactInfo(int m)
static char const * GetLibraryRelease(void)
double MoveAxis(std::vector< int >const &axes, bool sequ=true)
This file contains settings to make the SDHLibrary compile on differen systems:
char dsa_rs_device[MAX_DEV_LENGTH]
A data structure describing a full tactile sensor frame read from the remote DSACON32m controller...
std::vector< double > GetAxisActualAngle(std::vector< int > const &axes)
std::vector< double > GetAxisMaxAngle(std::vector< int > const &axes)
This file contains some basic definitions (defines, macros, datatypes)
class for command line option parsing holding option parsing results
Class to create an updater thread for continuously updating tactile sensor data.
virtual bool IsGrasped(void)
void interrupt()
interrupt the updater thread
USING_NAMESPACE_SDH NAMESPACE_SDH_START std::ostream * g_sdh_debug_log
void PDM(char const *fmt,...) SDH__attribute__((format(printf
unsigned long GetAgeOfFrame(sTactileSensorFrame *frame_p)