/* * This file is part of OpenKC * * Copyright(c) Matthias Schöpfer (mschoepf@techfak.uni-bielefeld.de) * http://opensource.cit-ec.de/projects/openkc * * This file may be licensed under the terms of of the * GNU Lesser General Public License Version 3 (the ``LGPL''), * or (at your option) any later version. * * Software distributed under the License is distributed * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either * express or implied. See the LGPL for the specific language * governing rights and limitations. * * You should have received a copy of the LGPL along with this * program. If not, go to http://www.gnu.org/licenses/lgpl.html * or write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * * The development of this software was supported by the * Excellence Cluster EXC 277 Cognitive Interaction Technology. * The Excellence Cluster EXC 277 is a grant of the Deutsche * Forschungsgemeinschaft (DFG) in the context of the German * Excellence Initiative. * */ #include #include #include #include /* #include */ #include #include #include #include #include "fri_okc_hostconfig.h" #ifdef HAVE_CONFIG_H #include #endif #ifdef HAVE_GETOPT_H #include #endif #define AC_AI 0x01 #define AC_CI 0x02 #define AC_GC 0x04 #define AC_JSD 0x08 #define AC_CSD 0x10 #define AC_JNTPOS 0x100 #define AC_CARTPOS 0x200 #define AC_SINE 0x1000 static void _print_help (const char* progname) { #ifdef HAVE_GETOPT_LONG printf ("%s Version %s\nCopyright by Matthias Sch\"opfer " "%s\n\n",progname,VERSION,PACKAGE_BUGREPORT); printf ("This is free software; see the source for copying conditions." "There is NO \nwarranty; not even for MERCHANTABILITY or FITNESS FOR A" " PARTICULAR PURPOSE.\n" "\nThe development of this software was supported by the Excellence" " Cluster EXC\n" "277 Cognitive Interaction Technology. The Excellence Cluster EXC " "277 is a grant\nof the Deutsche Forschungsgemeinschaft (DFG) in the " "context of the German\nExcellence Initiative.\n\n"); printf ("USAGE: %s [OPTIONS]\n",progname); printf ("--help/-h\t\t\t= print this help\n"); printf ("--axis_impedance/-x\t\t= cycle using axis impedance\n"); printf ("--cartesian_impedance/-c\t= cycle using cartesian impedance\n"); printf ("--gravcomp/-g\t\t\t= cycle using gravcomp-mode\n"); printf ("--all/-a\t\t\t= cycle through all modes\n"); printf ("--jstiffdamp/-j\t\t= use FRI_CMD_JNTSTIF/DAMP, if applicable\n"); printf ("--cstiffdamp/-s\t\t= use FRI_CMD_CARTSTIF/DAMP, if applicable\n"); printf ("--jntpos/-o\t\t= test with FRI_CMD_JNTPOS, if applicable\n"); printf ("--cartpos/-p\t\t= test with FRI_CMD_CARTPOS, if applicable\n"); printf ("--sine/-i\t\t= use sine movement\n"); #else printf ("%s Version %s\nCopyright by Matthias Sch\"opfer " "%s\n\n",progname,VERSION,PACKAGE_BUGREPORT); printf ("This is free software; see the source for copying conditions." "There is NO \nwarranty; not even for MERCHANTABILITY or FITNESS FOR A" " PARTICULAR PURPOSE.\n" "\nThe development of this software was supported by the Excellence" " Cluster EXC\n" "277 Cognitive Interaction Technology. The Excellence Cluster EXC " "277 is a grant\nof the Deutsche Forschungsgemeinschaft (DFG) in the " "context of the German\nExcellence Initiative.\n\n"); printf ("USAGE: %s [OPTIONS]\n",progname); printf ("-h\t\t = print this help\n"); printf ("-x\t\t= cycle using axis impedance\n"); printf ("-c\t\t= cycle using cartesian impedance\n"); printf ("-g\t\t= cycle using gravcomp-mode\n"); printf ("-a\t\t= cycle through all modes\n"); printf ("-j\t\t= use FRI_CMD_JNTSTIF/DAMP, if applicable\n"); printf ("-s\t\t= use FRI_CMD_CARTSTIF/DAMP, if applicable\n"); printf ("-o\t\ŧ= test with FRI_CMD_JNTPOS, if applicable\n"); printf ("-p\t\t= test with FRI_CMD_CARTPOS, if applicable\n"); printf ("-i\t\t= use sine movement\n"); #endif } int parse_args (int argc, char** argv) { int flags=0; #ifdef HAVE_GETOPT_LONG static struct option long_options[] = { {"help", no_argument, 0, 'h'}, {"axis_impedance", no_argument, 0, 'x'}, {"cartesian_impedance", no_argument, 0, 'c'}, {"gravcomp", no_argument, 0, 'g'}, {"all",no_argument,0,'a'}, {"jstiffdamp",no_argument,0,'j'}, {"cstiffdamp",no_argument,0,'s'}, {"jntpos",no_argument,0,'o'}, {"cartpos",no_argument,0,'p'}, {"sine",no_argument,0,'i'}, {0, 0, 0, 0} }; #endif char* optstring = "hxcgajsopi"; char* progname; int ret; #ifdef HAVE_GETOPT_LONG int longindex; #endif progname = malloc (strlen (argv[0])+1); if (NULL == progname) { perror ("malloc"); exit (EXIT_FAILURE); } strcpy (progname,argv[0]); #ifdef HAVE_GETOPT_LONG while ((ret = getopt_long_only (argc, argv, optstring, long_options, &longindex)) != -1) #else while ((ret = getopt (argc,argv,optstring)) != -1) #endif { if (ret == '?' || ret == 'h') { _print_help (progname); exit (EXIT_FAILURE); } switch (ret) { case 'x': flags |= AC_AI; break; case 'c': flags |= AC_CI; break; case 'g': flags |= AC_GC; break; case 'a': flags |= AC_AI|AC_CI|AC_GC; break; case 'j': flags |= AC_JSD; break; case 's': flags |= AC_CSD; break; case 'o': flags |= AC_JNTPOS; break; case 'p': flags |= AC_CARTPOS; break; case 'i': flags |= AC_SINE; break; default: _print_help (progname); exit (EXIT_FAILURE); } } free(progname); return flags; } static int keypressed (void) { fd_set readfds; struct timeval timeout; FD_ZERO (&readfds); FD_SET (0,&readfds); timeout.tv_sec = 0; timeout.tv_usec = 0; return select (1,&readfds,NULL,NULL,&timeout); } static double lin_trapezoid (double pos) { static const double peak = 0.0005; if (pos < 0.5) { return (pos*2.0)*peak; } else { if (pos < 1.0) return ((-2.0*(pos-0.5)*peak) + peak); } return 0.0; } static void move_all_joints_all_dirs (okc_handle_t* okc/* , double factor */) { double d; fri_float_t cycle_time; lbr_axis_t la; fri_float_t *p; int i; p = (fri_float_t*) &la; if (OKC_OK != okc_get_cycle_time (okc, 0, &cycle_time)) exit (EXIT_FAILURE); fprintf (stderr,"DEBUG: cycle time = %f\n",cycle_time); okc_axis_set_all_zero (&la); for (i = 0; i < 7; i++) { for (d = 0.0; d < 1.0; d += cycle_time) { p[i] = lin_trapezoid (d); if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); okc_sleep_cycletime (okc,0); } p[i] = 0.00; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); } for (i = 0; i < 7; i++) { for (d = 0.0; d < 1.0; d += cycle_time) { p[6-i] = -1.0* lin_trapezoid (d); if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); okc_sleep_cycletime (okc,0); } p[6 - i] = 0.00; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); } } static void make_sine_movement_from_fri_firstapp (okc_handle_t* okc) { fri_float_t cycle_time; lbr_axis_t la; fri_float_t *p; double timeCounter=0.0; int i; p = (fri_float_t*) &la; if (OKC_OK != okc_get_cycle_time (okc, 0, &cycle_time)) exit (EXIT_FAILURE); for (;;) { if (OKC_OK == okc_is_power_on (okc,0) && OKC_OK == okc_is_robot_in_command_mode (okc,0)) { timeCounter += cycle_time; for (i=0; i < 7; i++) p[i] = (float) cycle_time*sin( timeCounter * M_PI * 0.2) * (float)(10./180.*M_PI); if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); okc_sleep_cycletime (okc,0); } else { timeCounter = 0.0; } if (timeCounter > 10.0) break; } } static void move_tcp_all_dirs (okc_handle_t* okc) { int i; struct timespec ts; coords_t k; transform_t t; fri_float_t* p; p = (fri_float_t*) &k; okc_coords_set_all_zero (&k); for (i = 0; i < 6; i++) { fprintf (stderr,"Moving in direction %d\n",i); p[i] = 0.000024; okc_get_position_act (okc, 0, &t); printf ("Before movement:\n"); okc_print_transform (t); if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); ts.tv_sec = 2; ts.tv_nsec = 800000000; nanosleep (&ts,NULL); p[i] = 0.00; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); printf ("After movement: \n"); okc_get_position_act (okc, 0, &t); okc_print_transform (t); } for (i = 0; i < 6; i++) { fprintf (stderr,"Moving in direction %d\n",5-i); p[5 - i] = -0.000024; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); ts.tv_sec = 2; ts.tv_nsec = 800000000; nanosleep (&ts,NULL); p[5 - i] = 0.00; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); } } int main (int argc, char** argv) { okc_handle_t* okc; lbr_axis_t la, lb; coords_t ka, kb; int flags; int cmdFlags; int quality; flags = parse_args (argc,argv); okc = okc_start_server (OKC_HOST,OKC_PORT,OKC_MODE_SETTER_AXIS); if (NULL == okc) { fprintf (stderr,"Could not create OKC-Server\n"); exit (EXIT_FAILURE); } okc_axis_set_all_zero (&la); okc_axis_set_all_zero (&lb); okc_coords_set_all_zero (&ka); okc_coords_set_all_zero (&kb); while (OKC_OK != okc_is_robot_avail (okc,0)) sleep (1); okc_get_connection_quality (okc,0,&quality); while ((FRI_QUALITY_OK != quality) && (FRI_QUALITY_PERFECT != quality) ) { sleep(1); okc_get_connection_quality (okc,0,&quality); } cmdFlags = FRI_CMD_JNTPOS; if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags)) exit (EXIT_FAILURE); sleep (1); fprintf (stderr,"Got connect from robot. Requesting command mode . . . "); if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"\nFailed to request command mode\n"); exit (EXIT_FAILURE); } while (OKC_OK != okc_is_robot_in_command_mode (okc,0)) { if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"Some error occured. Bailing out\n"); exit (EXIT_FAILURE); } sleep(1); } fprintf (stderr,"done\n"); fprintf (stderr,"Waiting for drives to power on . . . "); while (OKC_OK != okc_is_power_on (okc,0)) sleep (1); fprintf (stderr,"done\n"); /* okc_print_robot_informations (okc,0); */ sleep (2); while (0 == keypressed()) { fprintf (stderr,"moving robot . . . "); if (AC_SINE == (flags & AC_SINE)) make_sine_movement_from_fri_firstapp (okc); else move_all_joints_all_dirs(okc); fprintf (stderr,"done\n"); if ((flags & AC_AI) == AC_AI) { la.a1 = 1500.0; la.a2 = 1500.0; la.a3 = 1025.0; la.a4 = 1025.0; la.a5 = 500.0; la.a6 = 1500.0; la.e1 = 1025.0; lb.a1 = 0.7; lb.a2 = 0.7; lb.a3 = 0.7; lb.a4 = 0.7; lb.a5 = 0.7; lb.a6 = 0.7; lb.e1 = 0.7; fprintf (stderr,"Dropping back to Monitor Mode . . . "); if (OKC_OK != okc_request_monitor_mode (okc,0)) exit (EXIT_FAILURE); cmdFlags = FRI_CMD_JNTPOS; if (AC_JSD == (flags & AC_JSD)) { cmdFlags |= FRI_CMD_JNTSTIFF; cmdFlags |= FRI_CMD_JNTDAMP; } if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSetting Axis Stiffness & Damping . . . "); if (OKC_OK != okc_set_axis_stiffness_damping (okc,0,la,lb)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSwitching to Axis Impedance . . . "); if (OKC_OK != okc_switch_to_axis_impedance (okc,0)) exit (EXIT_FAILURE); fprintf (stderr,"done\nGoing back to command mode . . . "); while (OKC_OK != okc_is_robot_in_command_mode (okc,0)) { if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"Some error occured. Bailing out\n"); exit (EXIT_FAILURE); } sleep(1); } fprintf (stderr,"done\n"); sleep (1); fprintf (stderr,"moving robot . . . "); if (AC_SINE == (flags & AC_SINE)) make_sine_movement_from_fri_firstapp (okc); else move_all_joints_all_dirs(okc); fprintf (stderr,"done\n"); } if ((flags & AC_CI) == AC_CI) { la.a1 = 2000.0; la.a2 = 2000.0; la.a3 = 2000.0; la.a4 = 2000.0; la.a5 = 2000.0; la.a6 = 2000.0; la.e1 = 2000.0; lb.a1 = 0.7; lb.a2 = 0.7; lb.a3 = 0.7; lb.a4 = 0.7; lb.a5 = 0.7; lb.a6 = 0.7; lb.e1 = 0.7; fprintf (stderr,"Dropping back to Monitor Mode . . . "); if (OKC_OK != okc_request_monitor_mode (okc,0)) exit (EXIT_FAILURE); cmdFlags = FRI_CMD_CARTPOS; if (AC_JSD == (flags & AC_JSD)) { cmdFlags |= FRI_CMD_JNTSTIFF; cmdFlags |= FRI_CMD_JNTDAMP; } if (AC_CSD == (flags & AC_CSD)) { cmdFlags |= FRI_CMD_CARTSTIFF; cmdFlags |= FRI_CMD_CARTDAMP; } /* cmdFlags = FRI_CMD_CARTPOS | FRI_CMD_JNTPOS | FRI_CMD_JNTSTIFF | FRI_CMD_JNTDAMP | FRI_CMD_CARTSTIFF | FRI_CMD_CARTDAMP; */ if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSetting Axis Stiffness & Damping . . . "); if (OKC_OK != okc_set_axis_stiffness_damping (okc,0,la,lb)) exit (EXIT_FAILURE); ka.x = 2000.0; ka.y = 2000.0; ka.z = 2000.0; ka.a = 200.0; ka.b = 200.0; ka.c = 200.0; kb.x = 0.7; kb.y = 0.7; kb.z = 0.7; kb.a = 0.7; kb.b = 0.7; kb.c = 0.7; fprintf (stderr,"done\nSetting Cartesian Stiffness & Damping . . . "); if (OKC_OK != okc_set_cp_stiffness_damping (okc,0,ka,kb)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSwitching to CP Impedance . . . "); if (OKC_OK != okc_switch_to_cp_impedance (okc,0)) exit (EXIT_FAILURE); sleep (1); fprintf (stderr,"done\nGoing back to command mode . . . "); okc_print_robot_informations (okc,0); while (OKC_OK != okc_is_robot_in_command_mode (okc,0)) { if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"Some error occured. Bailing out\n"); exit (EXIT_FAILURE); } sleep(1); } okc_print_robot_informations (okc,0); fprintf (stderr,"done\n"); sleep (1); fprintf (stderr,"moving robot . . . "); move_tcp_all_dirs (okc); fprintf (stderr,"done\n"); } if (((flags & AC_CI) == AC_CI) || ((flags & AC_AI) == AC_AI)) { fprintf (stderr,"Dropping back to Monitor Mode . . . "); if (OKC_OK != okc_request_monitor_mode (okc,0)) exit (EXIT_FAILURE); cmdFlags = FRI_CMD_JNTPOS; if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSwitching to Position control . . . "); if (OKC_OK != okc_switch_to_position (okc,0)) exit (EXIT_FAILURE); fprintf (stderr,"done\nGoing back to command mode . . . "); while (OKC_OK != okc_is_robot_in_command_mode (okc,0)) { if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"Some error occured. Bailing out\n"); exit (EXIT_FAILURE); } sleep(1); } fprintf (stderr,"done\n"); sleep(1); } if ((flags & AC_GC) == AC_GC) { fprintf (stderr,"Dropping back to Monitor Mode . . . "); if (OKC_OK != okc_request_monitor_mode (okc,0)) exit (EXIT_FAILURE); fprintf (stderr,"done\nSwitching to GRAVCOMP . . . "); if (OKC_OK != okc_switch_to_gravcomp (okc,0)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); sleep (5); cmdFlags = FRI_CMD_JNTPOS; if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags)) exit (EXIT_FAILURE); fprintf (stderr,"Switching to Position control . . . "); if (OKC_OK != okc_switch_to_position (okc,0)) exit (EXIT_FAILURE); fprintf (stderr,"done\nGoing back to command mode . . . "); while (OKC_OK != okc_is_robot_in_command_mode (okc,0)) { if (OKC_OK != okc_request_command_mode (okc,0)) { fprintf (stderr,"Some error occured. Bailing out\n"); exit (EXIT_FAILURE); } sleep(1); } fprintf (stderr,"done\n"); } } fprintf (stderr,"Exiting . . . "); okc_stop_server (okc); fprintf (stderr,"done\n"); return 0; }