/*****************************************************************************
******************************************************************************
Function Name : init_test_sine_task
Date : Dec. 1997
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_test_sine_task(void)
{
int j, i;
char string[100];
double max_range=0;
int ans;
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("New task can only be run if no other task is running!\n");
return FALSE;
}
/* allow or speed adjustment */
get_double("Frequency Multiplier",speed,&speed);
/* enable inverse dynamics control */
get_int("Which InvDyn: NewtonEuler=1 ArtBody=2",which_invdyn,&which_invdyn);
/* read the script for this task */
if (!read_sine_script())
return FALSE;
/* go to a save posture */
bzero((char *)&(target[1]),n_dofs*sizeof(target[1]));
for (i=1; i<=n_dofs; ++i) {
target[i].th = off[i];
for (j=1; j<=n_sine[i]; ++j) {
target[i].th += amp[i][j]*sin(phase[i][j]);
}
}
if (!go_target_wait_ID(target))
return FALSE;
/* switch the servo mode */
if (invdyn_servo_flag)
setServoMode(INVDYNSERVO);
else
setServoMode(MOTORSERVO);
/* do we really want to do this task? */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
task_time = 0.0;
scd();
return TRUE;
}
static int init_teacher_task ( void )
{
startFlag=1;
unifyFlag=1;
fileNameIndex++;
rest_joint_state[1].th = 1.8;
rest_joint_state[2].th = -0.2;
rest_joint_state[3].th = -0.1;
rest_joint_state[4].th = 1.8;
rest_joint_state[5].th = -1.57;
rest_joint_state[6].th = 0.1;
rest_joint_state[7].th = 0.3;
/*start_joint_state[1].th = 1.8;
start_joint_state[2].th = -0.2;
start_joint_state[3].th = 0.3;
start_joint_state[4].th = 1.8;
start_joint_state[5].th = -1.57;
start_joint_state[6].th = 0.1;
start_joint_state[7].th = 0.3;*/
int i,j;
for (i=1;i<=N_DOFS;i++)
{
rest_joint_state[i].thd=0;
rest_joint_state[i].thdd=0;
//start_joint_state[i].thd=0;
//start_joint_state[i].thdd=0;
}
/*for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].x[j]);printf("\n");
for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].xd[j]);printf("\n");*/
//saveJoint_state(1, 1, 1, 1, 1, 1);
if (!go_target_wait_ID(rest_joint_state))
return FALSE;
/*for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].x[j]); printf("\n");
for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].xd[j]); printf("\n");*/
//saveJoint_state(1, 1, 1, 1, 1, 1);
/*if (!go_target_wait_ID(start_joint_state))
return FALSE;
for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].x[j]); printf("\n");
for(j=_X_; j<=_Z_; j++)
printf("%.3f\t",cart_state[1].xd[j]); printf("\n");*/
//saveJoint_state(1, 1, 1, 1, 1, 1);
printf("\nball data and human demonstration are recording...\n\n");
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_turing_record_task
Date : June 2014
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_turing_record_task(void)
{
int j, i;
int ans;
char string[100];
static int firsttime = TRUE;
if (firsttime){
firsttime = FALSE;
//freq = 0.1; // frequency
//amp = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
/* add trigger signal to virtual oscilloscope:
add the variable name to prog/masterUser/prefs/task_default.osc*/
updateOscVars();
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
// start data collection
//scd();
//printf("Data collection started\n");
return TRUE;
}
int SampleTask::initialize()
{
start_time_ = 0.0;
static int firsttime = TRUE;
if (firsttime){
firsttime = FALSE;
freq_ = 0.1; // frequency
amp_ = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target_[1]),N_DOFS*sizeof(target_[1]));
for (int i=1; i<=N_DOFS; i++)
{
target_[i].th = joint_default_state[i].th;
}
target_[RA_J3].th=PI/4;
target_[LA_J3].th=PI/4;
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target_))
{
return FALSE;
}
// ready to go
int ans = 999;
while (ans == 999) {
if (!get_int(const_cast<char*>("Enter 1 to start or anthing else to abort ..."),ans,&ans))
{
return FALSE;
}
}
// only go when user really types the right thing
if (ans != 1)
{
return FALSE;
}
start_time_ = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n", start_time_, task_servo_time);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_sample_trigger
Date : Dec. 1997
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_sample_trigger(void)
{
int j, i;
int ans;
static int firsttime = TRUE;
if (firsttime){
firsttime = FALSE;
freq0 = 0.5; // frequency
freq1 = 0.9;
freq2 = 1.4;
amp0 = 1.0/2; // amplitude
amp1 = 3.0/2;
amp2 = 6.0/2;
}
// prepare going to the default posture
/*
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
*/
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
return TRUE;
}
static int
init_rosrt_test_task(void)
{
int j, i;
int ans;
static int firsttime = TRUE;
ros::init(a,null,"ros_rt_test_subscriber");
init_ros();
if (firsttime){
firsttime = FALSE;
freq = 0.1; // frequency
amp = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_ranger_task
Date : Dec. 1997
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_ranger_task(void)
{
int j, i;
int ans;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
create_contact_objects();
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// draw the ranger
float b[N_CART] = {0.,1.0,-0.95};
sendUserGraphics("rangerScenario",b,N_CART*sizeof(float));
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_elbow_perturbation
Date : January 2015
Remarks:
initialization for task
******************************************************************************
Parameters: (i/o = input/output)
none
*****************************************************************************/
static int
init_elbow_perturbation(void)
{
int j, i;
int ans;
// go to a save posture
bzero((char *)&(goto_state[1]),N_DOFS*sizeof(goto_state[1]));
for (i=1; i<=N_DOFS; ++i)
{
goto_state[i].th = joint_default_state[i].th;
}
if (!go_target_wait_ID(goto_state))
{
return FALSE;
}
// initialize desired joint drag state
for (i=1; i<=N_DOFS; ++i)
{
joint_des_drag_state[i].th = joint_state[i].th;
}
// zero the filters
for (i=1; i<=N_CART; ++i)
{
ff[i].cutoff = 40;
for (j=0; j<=FILTER_ORDER; ++j)
{
ff[i].raw[j] = ff[i].filt[j] = 0;
}
}
// initialize filter variables
for(i=1; i<=N_DOFS; i++)
{
for(j=1; j<=NZEROS; j++)
{
xv[i][j]=0.0;
xa[i][j]=0.0;
}
for(j=1; j<=NPOLES; j++)
{
yv[i][j]=0.0;
ya[i][j]=0.0;
}
}
// ready to go
ans = 999;
while (ans == 999)
{
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
{
return FALSE;
}
}
if (ans != 1)
{
return FALSE;
}
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
// start data collection
scd();
printf("Data collection started\n");
return TRUE;
}
int dmpTask::initialize()
{
double freq;
int i;
int ans = 999;
start_time_ = 0.0;
// Make sure that everything is clear
if (vnames_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) vnames_CBi[i]);
free((void *) vnames_CBi);
vnames_CBi = NULL;
}
if (units_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) units_CBi[i]);
free((void *) units_CBi);
units_CBi = NULL;
}
if (CBi_traj != NULL)
{
my_free_matrix(CBi_traj, 1, n_CBi, 1, dof_CBi);
CBi_traj = NULL;
}
if (vnames_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) vnames_Kinect[i]);
free((void *) vnames_Kinect);
vnames_Kinect = NULL;
}
vnames_Kinect = NULL;
if (units_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) units_Kinect[i]);
free((void *) units_Kinect);
units_Kinect = NULL;
}
if (Kinect_traj != NULL)
{
my_free_matrix(Kinect_traj, 1, n_Kinect, 1, dof_Kinect);
Kinect_traj = NULL;
}
if (DMP_object != NULL)
{
delete DMP_object;
DMP_object = NULL;
}
// Read robot trajectory
// mrdplot_convert(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi);
mrdplot_read(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi);
if (!set_active_dofs(vnames_CBi, dof_CBi, active_dofs))
return FALSE;
printf("%d active DoFs:", active_dofs[0]);
for (i = 2; i <= active_dofs[0]+1; i++)
{
if (!((i-2)%8))
printf("\n");
printf("%s, ", vnames_CBi[i]);
}
printf("\n\n");
// Read Kinect trajectory
// mrdplot_convert(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect);
mrdplot_read(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect);
// Filter Kinect trajectory if desired
if (Kinect_traj != NULL && filter_Kinect_data)
{
if (!init_filters())
return FALSE;
for (int i = 1; i <= 19; i++)
fth[i].cutoff = 9;
for (int i = 1; i <= 19; i++)
{
fth[i].raw[0] = fth[i].raw[1] = fth[i].raw[2] = Kinect_traj[1][i+1];
fth[i].filt[0] = fth[i].filt[1] = fth[i].filt[2] = Kinect_traj[1][i+1];
}
for (int j = 1; j <= n_Kinect; j++)
for (int i = 1; i <= 19; i++)
Kinect_traj[j][i+1] = filt(Kinect_traj[j][i+1], &fth[i]);
}
// Compute DMP for robot trajectory
if (DMP_object != NULL)
delete DMP_object;
// DMP_object = new DMP_structure(DMP_file);
DMP_object = new DMP_structure(active_dofs[0], 50, 2.0, 12.0, 3.0);
printf("\n");
if (!DMP_object->example_Matrix(CBi_traj, n_CBi, dof_CBi))
return FALSE;
DMP_object->DMP_estimate(0);
// DMP_object->DMP_param_print();
printf("\n");
// Initialize DMP integration
for (int i = 1; i <= active_dofs[0]; i++)
{
initial_positions[i] = CBi_traj[1][i+1];
initial_velocities[i] = 0.0;
}
DMP_object->set_initial_DMP_state(&(initial_positions[1]), &(initial_velocities[1]));
// include this file to define contact points (needed to compute center of pressure)
#include "LEKin_contact.h"
// prepare going to the default posture
bzero((char *)&(target_[1]),N_DOFS*sizeof(target_[1]));
for (i = 1; i <= N_DOFS; i++)
target_[i] = joint_default_state[i];
target_[L_EB].th = target_[R_EB].th = 0.05;
target_[B_TFE].th = 0.2;
for (int i = 1; i <= active_dofs[0]; i++)
{
target_[active_dofs[i]].th = initial_positions[i];
target_[active_dofs[i]].thd = 0.0;
target_[active_dofs[i]].thdd = 0.0;
target_[active_dofs[i]].uff = 0.0;
}
bool there = true;
for (int i = 1; i <= B_HR; i++)
if (fabs(target_[i].th - joint_des_state[i].th) > 1.0e-3)
{
there = false;
break;
}
// go to the target using inverse dynamics (ID)int SampleTask::changeParameters()
if (!there)
{
if (!go_target_wait_ID(target_))
{
return FALSE;
}
}
// ready to go
while (ans == 999)
{
if (!get_int(const_cast<char*>("Enter 1 to start or anthing else to abort ..."), ans, &ans))
{
return FALSE;
}
}
// only go when user really types the right thing
if (ans != 1)
{
return FALSE;
}
start_time_ = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n", start_time_, task_servo_time);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Description: initialization for task
Parameters:
none
*****************************************************************************/
static int init_mpc_task(void) {
int i,j;
int ans;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
lookupTable = my_matrix(1, LOOKUP_TABLE_SIZE, 1, LOOKUP_COLUMN_SIZE);
}
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("Task can only be run if no other task is running!\n");
return FALSE;
}
get_int("Use simulated ball?", FALSE, &simulation);
if(simulation) {
printf("Testing table tennis algorithms on the simulator...\n");
}
/* go to a save posture */
bzero((char *)&(init_joint_state[1]), N_DOFS * sizeof(init_joint_state[1]));
init_joint_state[1].th = 1.0;
init_joint_state[2].th = -0.2;
init_joint_state[3].th = -0.1;
init_joint_state[4].th = 1.8;
init_joint_state[5].th = -1.57;
init_joint_state[6].th = 0.1;
init_joint_state[7].th = 0.3;
for (i = 1; i <= N_DOFS; i++) {
current_joint_state[i].th = init_joint_state[i].th;
current_joint_state[i].thd = init_joint_state[i].thd;
}
go_target_wait(init_joint_state);
if (!go_target_wait_ID(init_joint_state))
return FALSE;
printf("Loading lookup table...\n");
load_vec_into_mat(lookupTable, LOOKUP_TABLE_SIZE,
LOOKUP_COLUMN_SIZE, LOOKUP_TABLE_NAME);
// just testing
if (simulation) {
// initialize ball cannon position
init_ball_cannon();
reset_sim_ball();
}
else {
// real robot mode
}
/*get_int("Friction Compensation?", FALSE, &friction_comp);
if (friction_comp) {
printf("Loading stiction model...\n");
loadStictionModel("./prefs/stiction.pref");
}*/
// for real setup
setDefaultEndeffector();
endeff[RIGHT_HAND].x[_Z_] = .3;
// indices for ball and robot trajectory
trj_time = 0; // clear planned trajectory
// debug task if you like
//debug_task();
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans, &ans))
return FALSE;
}
if (ans != 1)
return FALSE;
busy = FALSE;
// turn off real time
changeRealTime(TRUE);
return TRUE;
}
static int
init_fpga_task(void)
{
int j, i;
int ans;
static int firsttime = TRUE;
//enter code
/* get home path */
homePath= getenv ("HOME");
//////////////////////////////////////////////////////////////////////////
// Create a MMAP
//////////////////////////////////////////////////////////////////////////
#ifdef WIN32
/*// Create a 4K memory-mapped file (MMF)
hFile = CreateFile((LPCTSTR) "input.dat",
GENERIC_WRITE | GENERIC_READ,
FILE_SHARE_READ | FILE_SHARE_WRITE, NULL,
OPEN_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if(hFile == INVALID_HANDLE_VALUE)
{
// Failed to create file
return 1;
}
map = CreateFileMapping(hFile, NULL, PAGE_READWRITE, 0, 1024, "MMAPShmem");
g_fpga2robot = (char *) MapViewOfFile (map, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, 0);
*/
#else
printf("buffer_size: %d\n\n", statbuf.st_size);
int fd_i;
char* fgpa2robot_FilePath;
fgpa2robot_FilePath = strncat(homePath, "/prog/masterUser/src/fpga2robot.dat", 100); //max number of characers to be concatinated :100
if((fd_i = open(fgpa2robot_FilePath, O_RDWR)) == -1)
{
printf("Couldn't open 'fpga2robot.dat'\n");
}
g_fpga2robot = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_SHARED, fd_i, 0);
if (g_fpga2robot == MAP_FAILED) {
close(fd_i);
perror("Error mmapping the file");
exit(EXIT_FAILURE);
}
int fd_o;
if((fd_o = open("/home/eric/prog/masterUser/src/robot2fpga.dat", O_RDWR)) == -1)
{
printf("Couldn't open 'robot2fpga.dat'\n");
}
lseek(fd_o, 0, SEEK_SET);
g_robot2fpga = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_SHARED, fd_o, 0);
if (g_robot2fpga == MAP_FAILED) {
close(fd_o);
perror("Error mmapping the file");
exit(EXIT_FAILURE);
}
#endif
if(g_fpga2robot != NULL)
{
printf("Wrapper has created a MMAP for file 'fpga2robot.dat'\n");
}
if(g_robot2fpga != NULL)
{
printf("Wrapper has created a MMAP for file 'robot2fpga.dat'\n");
}
/* find size of input file */
if (fstat (fd_i, &statbuf) < 0)
err_sys ("fstat error");
system("/home/eric/prog/masterUser/src/py/py_mmap.py &");
//end code
if (firsttime){
firsttime = FALSE;
freq = 0.1; // frequency
amp = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_sine_task
\date Dec. 1997
\remarks
initialization for task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
init_sine_task(void)
{
int j, i;
char string[100];
double max_range=0;
int ans;
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("New task can only be run if no other task is running!\n");
return FALSE;
}
/* allow or speed adjustment */
get_double("Frequency Multiplier",speed,&speed);
/* allow inverse dynamics */
get_int("Use Inverse Dynamics",invdyn,&invdyn);
/* read the script for this task */
if (!read_sine_script())
return FALSE;
/* transient multiplier: ramps up the amplitude in one period to
avoid discontinuous motor commands */
trans_mult = 0.0;
/* what is the longest period? this is what we use to ramp up trans_mult to one */
trans_period = 0.0;
for (i=1; i<=n_dofs; ++i)
for (j=1; j<=n_sine[i]; ++j)
if (freq[i][j] > 0)
if (1./freq[i][j] > trans_period)
trans_period = 1./freq[i][j];
/* go to a save posture */
bzero((char *)&(target[1]),n_dofs*sizeof(target[1]));
for (i=1; i<=n_dofs; ++i) {
target[i].th = off[i];
for (j=1; j<=n_sine[i]; ++j) {
target[i].th += trans_mult*amp[i][j]*sin(phase[i][j]);
}
}
if (invdyn) {
if (!go_target_wait_ID(target))
return FALSE;
} else {
if (!go_target_wait(target))
return FALSE;
}
/* do we really want to do this task? */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
task_time = 0.0;
scd();
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_turing_playback_task
Date : June 2014
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_turing_playback_task(void)
{
int j, i;
int ans;
char string[100];
static int firsttime = TRUE;
if (firsttime){
firsttime = FALSE;
//freq = 0.1; // frequency
//amp = 0.5; // amplitude
}
// prepare going to the default posture
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; i++)
target[i] = joint_default_state[i];
// go to the target using inverse dynamics (ID)
if (!go_target_wait_ID(target))
return FALSE;
// add trigger to data collection
sprintf(string,"emgTrig");
addVarToCollect((char *)&(emgTrig),string,"V", DOUBLE,TRUE);
/* add trigger signal to virtual oscilloscope:
add the variable name to prog/masterUser/prefs/task_default.osc*/
updateOscVars();
// clear start data collection flag
//dataFlag = 0;
// input file option to ask
// list files in directory
DIR *d;
struct dirent *dir;
d = opendir("selected_vars/.");
if (d)
{
while ((dir = readdir(d)) != NULL)
{
printf("%s\n", dir->d_name);
}
closedir(d);
}
// get user input
char file_selected_vars[80];
printf ("Enter a file name to open: ");
scanf ("%s", file_selected_vars);
// read position array from txt.
char path[100] = "selected_vars/";
strcat (path, file_selected_vars);
FILE *fp = fopen(path, "r");
if (!fp) {
fprintf(stderr, "Can't open file.\n");
//exit(EXIT_FAILURE);
}
/*
int *a,temp,count=0;
a=(int *)malloc(10*sizeof(int));
temp=fgetc(fp);
while(!feof(fp))
{
if(temp!=44&&temp!=10&&temp!=32)
{
count++;
a[count]=temp;
printf("%c\n", a[count]);
}
temp=fgetc(fp);
}
fclose(fp);
*/
// read the number of lines in the text file.
/*
char line[1024];
int NumberOfLines = 0;
while( fgets(line,sizeof(line),fp) != NULL)
NumberOfLines++;
printf("the number of lines : %i\n", NumberOfLines);
*/
double temp;
int p = 0;
//read lines
const char s[2] = ",";
char *token;
int m;
if(fp != NULL)
{
char line[100]; //10 is enough per line
while(fgets(line, sizeof line, fp) != NULL)
{
token = strtok(line, s);
for(m=0;m<2;m++)
{
if(m==0)
{
printf("%s\t",token);
token = strtok(NULL,s);
pos2DArray[p][0] = atof(token);
} else {
printf("%f\n",atof(token));
pos2DArray[p][1] = atof(token);
}
}
p++;
}
fclose(fp);
}
/*
while (!feof(fp)){ //repeat until end of file
if (p>ARRAYLEN) { p = ARRAYLEN; break;}
fscanf(fp,"%f",&temp);
pos2DArray[p][1] = temp; // time
fscanf(fp,"%f",&temp);
pos2DArray[p][2] = temp; // pos
p++;
printf("p is...%i\n", p);
}
fclose(fp);
*/
// print the trajectory
int l;
for (l = 0; l < ARRAYLEN ; l++) {
// printf("pos2dArray %i, %f\n", l, pos2DArray[l][0]);
}
// ready to go
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
// only go when user really types the right thing
if (ans != 1)
return FALSE;
start_time = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n",
start_time, task_servo_time);
printf("Input var = %i\n",ivar);
// start data collection
scd();
printf("Data collection started\n");
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_goto_cart_task
\date Dec. 1997
\remarks
initialization for task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
init_goto_cart_task(void)
{
int j, i;
char string[100];
double max_range=0;
int ans;
double aux;
int flag = FALSE;
int iaux;
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("Goto task can only be run if no other task is running!\n");
return FALSE;
}
/* zero the filters */
for (i=1; i<=n_dofs; ++i)
for (j=0; j<=FILTER_ORDER; ++j)
fthdd[i].raw[j] = fthdd[i].filt[j] = 0;
/* initialize some variables */
init_vars();
time_step = 1./(double)task_servo_rate;
if (!special_flag) {
/* go to the same target as the current one, but with ID */
for (i=1; i<=n_dofs; ++i)
target[i] = joint_des_state[i];
if (!go_target_wait_ID(target))
return FALSE;
/* input the movement speed */
get_double("Movement Time?",movement_time,&movement_time);
if (movement_time < 0.2)
movement_time = 0.2;
tau = movement_time;
/* input the cartesian goal */
for (i=1; i<=n_endeffs; ++i) {
sprintf(string,"%s_x Status",cart_names[i]);
iaux = cstatus[(i-1)*6+ _X_];
get_int(string,cstatus[(i-1)*6+ _X_],&(cstatus[(i-1)*6+ _X_]));
if (cstatus[(i-1)*6+ _X_ ]) {
if (!iaux)
ctarget[i].x[_X_] = cart_des_state[i].x[_X_];
flag = TRUE;
sprintf(string,"%s_x Target",cart_names[i]);
get_double(string,ctarget[i].x[_X_],&aux);
ctarget[i].x[_X_] = aux;
}
sprintf(string,"%s_y Status",cart_names[i]);
iaux = cstatus[(i-1)*6+ _Y_];
get_int(string,cstatus[(i-1)*6+ _Y_],&(cstatus[(i-1)*6+_Y_]));
if (cstatus[(i-1)*6+_Y_]) {
if (!iaux)
ctarget[i].x[_Y_] = cart_des_state[i].x[_Y_];
flag = TRUE;
sprintf(string,"%s_y Target",cart_names[i]);
get_double(string,ctarget[i].x[_Y_],&aux);
ctarget[i].x[_Y_] = aux;
}
sprintf(string,"%s_z Status",cart_names[i]);
iaux = cstatus[(i-1)*6+ _Z_];
get_int(string,cstatus[(i-1)*6+ _Z_],&(cstatus[(i-1)*6+_Z_]));
if (cstatus[(i-1)*6+_Z_]) {
if (!iaux)
ctarget[i].x[_Z_] = cart_des_state[i].x[_Z_];
flag = TRUE;
sprintf(string,"%s_z Target",cart_names[i]);
get_double(string,ctarget[i].x[_Z_],&aux);
ctarget[i].x[_Z_] = aux;
}
}
if (!flag)
return FALSE;
/* check whether target is reachable */
SL_DJstate js[n_dofs+1];
for (i=1; i<=n_dofs; ++i)
js[i] = joint_des_state[i];
if (!checkIKTarget(js, &base_state, &base_orient, endeff, ctarget, cstatus, 1000)) {
printf("Target was reduced to reachable limits:\n");
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
if (cstatus[(j-1)*6+i]) {
printf("%s.%d = %f \n",cart_names[j],i,ctarget[j].x[i]);
}
}
}
}
}
/* the cnext state is the desired state as seen form this program */
for (i=1; i<=n_endeffs;++i) {
cnext[i] = cart_des_state[i];
}
if (!special_flag) {
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
scd();
} else {
special_flag = FALSE;
}
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : init_gravityComp_task
Date : Dec. 1997
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_gravityComp_task(void)
{
int ans;
int j,i;
static int firsttime = TRUE;
if (firsttime == TRUE) {
xd = my_vector(1,3);
B = my_matrix(1,3,1,3);
J7T = my_matrix(1,7,1,3);
u_field = my_vector(1,7);
B[1][1] = -10.1;
B[1][2] = -11.2;
B[1][3] = 0.0;
B[2][1] = -11.2;
B[2][2] = -11.1;
B[2][3] = 0.0;
B[3][1] = 0.0;
B[3][2] = 0.0;
B[3][3] = 1.0;
//for (i=1;i<=3;i++) {
// for (j=1;j<=3;j++) {
//if (i==j) {
// B[i][j] = -1.0;
//}
//else {
// B[i][j] = 0.0;
//}
//}
//}
firsttime = FALSE;
}
/* go to a save posture */
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; ++i) {
target[i].th = joint_default_state[i].th;
}
if (!go_target_wait_ID(target))
return FALSE;
/* initialize filter variables */
for(i=1; i<N_DOFS; i++) {
for(j=1; j<NZEROS; j++) {
xv[i][j]=0.0;
xa[i][j]=0.0;
}
for(j=1; j<NPOLES; j++) {
yv[i][j]=0.0;
ya[i][j]=0.0;
}
}
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
return TRUE;
}
