/*
* 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 <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include <time.h>
/* #include <unistd.h> */
#include <sys/time.h>
#include <errno.h>
#include <fri_okc_types.h>
#include <fri_okc_comm.h>
#include "fri_okc_hostconfig.h"
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#ifdef HAVE_GETOPT_H
#include <getopt.h>
#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;
}