/* * 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 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"); #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"); #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", required_argument, 0, 'c'}, {"gravcomp", required_argument, 0, 'g'}, {"all",required_argument,0,'a'}, {0, 0, 0, 0} }; #endif char* optstring = "hxcga"; 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; 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 void move_all_robots_joints_all_dirs(okc_handle_t* okc) { coords_t k; lbr_axis_t la; struct timespec ts; double *p; int i; p = (double*) &la; for (i = 0; i < 7; i++) { p[i] = 0.05; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_correction (okc,1,la)) exit (EXIT_FAILURE); ts.tv_sec = 0; ts.tv_nsec = 800000000; nanosleep (&ts,NULL); p[i] = 0.00; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_correction (okc,1,la)) exit (EXIT_FAILURE); } for (i = 0; i < 7; i++) { p[6 - i] = -0.05; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_correction (okc,1,la)) exit (EXIT_FAILURE); ts.tv_sec = 0; ts.tv_nsec = 800000000; nanosleep (&ts,NULL); p[6 - i] = 0.00; if (OKC_OK != okc_set_axis_correction (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_correction (okc,1,la)) exit (EXIT_FAILURE); } p = (double*) &k; for (i = 0; i < 6; i++) { p[i] = 0.1; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_coords_correction (okc,1,k)) exit (EXIT_FAILURE); ts.tv_sec = 0; ts.tv_nsec = 800000000; nanosleep (&ts,NULL); p[i] = 0.00; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_coords_correction (okc,1,k)) exit (EXIT_FAILURE); } for (i = 0; i < 6; i++) { p[5 - i] = -0.1; if (OKC_OK != okc_set_coords_correction (okc,0,k)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_coords_correction (okc,1,k)) exit (EXIT_FAILURE); ts.tv_sec = 0; 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); if (OKC_OK != okc_set_coords_correction (okc,1,k)) exit (EXIT_FAILURE); } } int main (int argc, char** argv) { okc_handle_t* okc; lbr_axis_t la; coords_t k; int flags; 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_coords_set_all_zero (&k); while (OKC_OK != okc_is_robot_avail (okc,0) || OKC_OK != okc_is_robot_avail (okc,1)) sleep (1); sleep (2); while (0 == keypressed()) { move_all_robots_joints_all_dirs(okc); if ((flags & AC_AI) == AC_AI) { la.a1 = 1500.0; la.a2 = 1500.0; la.a3 = 15.0; la.a4 = 15.0; la.a5 = 500.0; la.a6 = 1500.0; la.e1 = 15.0; if (OKC_OK != okc_set_axis_stiffness (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_stiffness (okc,1,la)) exit (EXIT_FAILURE); la.a1 = 0.7; la.a2 = 0.7; la.a3 = 0.7; la.a4 = 0.7; la.a5 = 0.7; la.a6 = 0.7; la.e1 = 0.7; if (OKC_OK != okc_set_axis_damping (okc,0,la)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_axis_damping (okc,1,la)) exit (EXIT_FAILURE); fprintf (stderr,"Switching to Axis Impedance . . . "); if (OKC_OK != okc_switch_to_axis_impedance (okc,0)) exit (EXIT_FAILURE); if (OKC_OK != okc_switch_to_axis_impedance (okc,1)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); sleep (1); move_all_robots_joints_all_dirs (okc); } if ((flags & AC_CI) == AC_CI) { k.x = 2000.0; k.y = 200.0; k.z = 1000.0; k.a = 100.0; k.b = 100.0; k.c = 100.0; if (OKC_OK != okc_set_cp_stiffness (okc,0,k)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_cp_stiffness (okc,1,k)) exit (EXIT_FAILURE); k.x = 0.7; k.y = 0.3; k.z = 0.7; k.a = 0.7; k.b = 0.7; k.c = 0.7; if (OKC_OK != okc_set_cp_damping (okc,0,k)) exit (EXIT_FAILURE); if (OKC_OK != okc_set_cp_damping (okc,1,k)) exit (EXIT_FAILURE); fprintf (stderr,"Switching to CP Impedance . . . "); if (OKC_OK != okc_switch_to_cp_impedance (okc,0)) exit (EXIT_FAILURE); if (OKC_OK != okc_switch_to_cp_impedance (okc,1)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); sleep (1); move_all_robots_joints_all_dirs (okc); } if (((flags & AC_CI) == AC_CI) || ((flags & AC_AI) == AC_AI)) { fprintf (stderr,"Switching to Position control . . . "); if (OKC_OK != okc_switch_to_position (okc,0)) exit (EXIT_FAILURE); if (OKC_OK != okc_switch_to_position (okc,1)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); sleep (1); } if ((flags & AC_GC) == AC_GC) { fprintf (stderr,"Switching to GRAVCOMP . . . "); if (OKC_OK != okc_switch_to_gravcomp (okc,0)) exit (EXIT_FAILURE); if (OKC_OK != okc_switch_to_gravcomp (okc,1)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); sleep (5); fprintf (stderr,"Switching to Position control . . . "); if (OKC_OK != okc_switch_to_position (okc,0)) exit (EXIT_FAILURE); if (OKC_OK != okc_switch_to_position (okc,1)) exit (EXIT_FAILURE); fprintf (stderr,"done\n"); } } fprintf (stderr,"Exiting . . . "); okc_stop_server (okc); fprintf (stderr,"done\n"); return 0; }