/***************************************************************************
* C_interface_for_robot_control.cpp *
* ------------------- *
* copyright : (C) 2013 by Richard R. Carrillo and Niceto R. Luque *
* email : rcarrillo at ugr.es *
***************************************************************************/
/***************************************************************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 3 of the License, or *
* (at your option) any later version. *
* *
***************************************************************************/
/*!
* \file C_interface_for_robot_control.cpp
*
* \author Richard R. Carrillo
* \author Niceto R. Luque
* \date 7 of November 2013
*
* This file defines the interface functions to access EDLUT's functionality.
*/
#include <time.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "../../include/simulation/Simulation.h"
#include "../../include/spike/Network.h"
#include "../../include/spike/InputSpike.h"
#include "../../include/communication/ArrayOutputSpikeDriver.h"
#include "../../include/communication/FileInputSpikeDriver.h"
#include "../../include/communication/FileOutputSpikeDriver.h"
#include "../../include/communication/FileOutputWeightDriver.h"
#include "../../include/simulation/EventQueue.h"
#include "../../include/spike/EDLUTFileException.h"
#include "../../include/spike/EDLUTException.h"
#include "../../include/communication/ConnectionException.h"
#include "../../include/interface/C_interface_for_robot_control.h"
#include "../../include/simulation/RealTimeRestriction.h"
#include "../../include/openmp/openmp.h"
///////////////////////////// SIMULATION MANAGEMENT //////////////////////////
long N_one_slot_internal_spikes2;
extern "C" Simulation *create_neural_simulation(const char *net_file, const char *input_weight_file, const char *input_spike_file, const char *output_weight_file, const char *output_spike_file, double weight_save_period, int number_of_openmp_queues, int number_of_openmp_threads)
{
Simulation *neural_sim; // Neural-simulator object (EDLUT)
try
{
neural_sim=new Simulation(net_file, input_weight_file, -1.0, 0.0, number_of_openmp_queues, number_of_openmp_threads); // End-simulation time not specified
if(input_spike_file)
neural_sim->AddInputSpikeDriver(new FileInputSpikeDriver(input_spike_file));
if(output_weight_file) // Driver to save neural-network weights
{
neural_sim->AddOutputWeightDriver(new FileOutputWeightDriver(output_weight_file));
neural_sim->SetSaveStep((float)weight_save_period);
}
neural_sim->AddOutputSpikeDriver(new ArrayOutputSpikeDriver()); // OutputSpikeDriver used to send activity to the robot interface. This output driver must be inserted first
if(output_spike_file) // Driver to write output activity
neural_sim->AddMonitorActivityDriver(new FileOutputSpikeDriver(output_spike_file,false)); // Neuron potentials are not written
neural_sim->InitSimulation();
}
catch(EDLUTException exc)
{
cerr << exc;
neural_sim=NULL;
}
return(neural_sim);
}
extern "C" void finish_neural_simulation(Simulation *neural_sim)
{
// EDLUT interface drivers
FileInputSpikeDriver *neural_activity_input_file;
FileOutputWeightDriver *neural_weight_output_file;
ArrayOutputSpikeDriver *neural_activity_output_array;
FileOutputSpikeDriver *neural_activity_output_file;
neural_activity_input_file=(FileInputSpikeDriver *)neural_sim->GetInputSpikeDriver(0);
if(neural_activity_input_file)
{
neural_sim->RemoveInputSpikeDriver(neural_activity_input_file);
delete neural_activity_input_file;
}
neural_weight_output_file=(FileOutputWeightDriver *)neural_sim->GetOutputWeightDriver(0);
if(neural_weight_output_file)
{
neural_sim->RemoveOutputWeightDriver(neural_weight_output_file);
delete neural_weight_output_file;
}
neural_activity_output_array=(ArrayOutputSpikeDriver *)neural_sim->GetOutputSpikeDriver(0); // The first output spike driver in the list is the ArrayOutputSpikeDriver
if(neural_activity_output_array)
{
neural_sim->RemoveOutputSpikeDriver(neural_activity_output_array); // remove the driver from the simulation output-driver list
delete neural_activity_output_array;
}
neural_activity_output_file=(FileOutputSpikeDriver *)neural_sim->GetMonitorActivityDriver(0); // The first monitor-activity driver in the list is the FileOutputSpikeDriver
if(neural_activity_output_file)
{
neural_sim->RemoveMonitorActivityDriver(neural_activity_output_file);
delete neural_activity_output_file;
}
delete neural_sim;
}
extern "C" int run_neural_simulation_slot(Simulation *neural_sim, double next_step_sim_time)
{
int ret=0;
if(omp_get_thread_num()==0){
N_one_slot_internal_spikes2=neural_sim->GetTotalSpikeCounter();
}
#pragma omp barrier
try{
neural_sim->RunSimulationSlot(next_step_sim_time);
#pragma omp barrier
if(omp_get_thread_num()==0){
N_one_slot_internal_spikes2=neural_sim->GetTotalSpikeCounter()-N_one_slot_internal_spikes2;
}
ret=0;
}
catch(ConnectionException exc){
cerr << exc << endl;
ret=1;
}
catch(EDLUTFileException exc){
cerr << exc << endl;
ret=exc.GetErrorNum();
}
catch(EDLUTException exc){
cerr << exc << endl;
ret=exc.GetErrorNum();
}
return(ret);
}
extern "C" void reset_neural_simulation(Simulation *neural_sim)
{
for(int i=0; i<neural_sim->GetNumberOfQueues(); i++){
neural_sim->GetQueue()->RemoveSpikes(i);
}
}
extern "C" void save_neural_weights(Simulation *neural_sim)
{
neural_sim->SaveWeights();
}
extern "C" long get_neural_simulation_spike_counter_for_each_slot_time()
{
return N_one_slot_internal_spikes2;
}
extern "C" long long get_neural_simulation_event_counter(Simulation *neural_sim)
{
return(neural_sim->GetSimulationUpdates());
}
extern "C" long long get_accumulated_heap_occupancy_counter(Simulation *neural_sim)
{
return(neural_sim->GetHeapAcumSize());
}
///////////////////////////// DELAY LINES FOR THE CONTROL LOOP //////////////////////////
void init_delay(struct delay *d, double del_time)
{
int nelem, npos;
if(del_time>MAX_DELAY_TIME)
del_time=MAX_DELAY_TIME;
d->length=(int)(del_time/SIM_SLOT_LENGTH+1.5); // +1 because one position of the line is wasted to return the oldest element. +0.5 to round the size
d->index=0;
for(npos=0;npos<d->length;npos++)
for(nelem=0;nelem<NUM_OUTPUT_VARS;nelem++) //NUM_JOINTS
d->buffer[npos][nelem]=0;
}
double *delay_line(struct delay *d, double *elem)
{
int nelem,next_index;
double *elems;
next_index=(d->index+1)%d->length; // oldest used element position
elems=d->buffer[next_index];
for(nelem=0;nelem<NUM_OUTPUT_VARS;nelem++)// NUM_JOINTS
d->buffer[d->index][nelem]=elem[nelem];
d->index=next_index; // make index point to the returned element
return(elems);
}
/////////////////////DELAY LINES FOR THE CONTROL LOOP PER JOINT NOT PER MICROZONE//////////////////////////
void init_delay_joint(struct delay_joint *d, double del_time)
{
int nelem, npos;
if(del_time>MAX_DELAY_TIME)
del_time=MAX_DELAY_TIME;
d->length_joint=(int)(del_time/SIM_SLOT_LENGTH+1.5); // +1 because one position of the line is wasted to return the oldest element. +0.5 to round the size
d->index_joint=0;
for(npos=0;npos<d->length_joint;npos++)
for(nelem=0;nelem<NUM_JOINTS;nelem++) //NUM_JOINTS
d->buffer_joint[npos][nelem]=0.0; }
double *delay_line_joint(struct delay_joint *d, double *elem)
{
int nelem,next_index;
double *elems;
next_index=(d->index_joint+1)%d->length_joint; // oldest used element position
elems=d->buffer_joint[next_index];
for(nelem=0;nelem<NUM_JOINTS;nelem++)// NUM_JOINTS
d->buffer_joint[d->index_joint][nelem]=elem[nelem];
d->index_joint=next_index; // make index point to the returned element
return(elems);
}
///////////////////////////// INPUT TRAJECTORY //////////////////////////
double gaussian_function(double a, double b, double c, double x)
{
return(a*exp(-(x-b)*(x-b)/(2*c*c)));
}
extern "C" void calculate_input_trajectory_sine_inverse(double *inp, double amplitude, double tsimul,double num_rep)
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
// sine trajectory (qd[0] = 0 and qd[1] = 0 and q[0] = 0 and q[1] = 2*pi)
inp[njoint]=-amplitude*sin(2.0*M_PI*tsimul*num_rep + njoint*M_PI/4.0);
inp[njoint+NUM_JOINTS]=-2.0*amplitude* M_PI*num_rep*cos(2*M_PI*tsimul*num_rep);
inp[njoint+2*NUM_JOINTS]=4.0*amplitude*M_PI*M_PI*num_rep*num_rep*sin(2.0*M_PI*num_rep*tsimul);
}
}
extern "C" void calculate_input_trajectory_sine(double *inp, double amplitude, double tsimul,double num_rep)
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
// sine trajectory (qd[0] = 0 and qd[1] = 0 and q[0] = 0 and q[1] = 2*pi)
inp[njoint]=amplitude*sin(2.0*M_PI*tsimul*num_rep + njoint*M_PI/4.0);
inp[njoint+NUM_JOINTS]=2.0*amplitude* M_PI*num_rep*cos(2*M_PI*tsimul*num_rep);
inp[njoint+2*NUM_JOINTS]=-4.0*amplitude*M_PI*M_PI*num_rep*num_rep*sin(2.0*M_PI*num_rep*tsimul);
}
}
extern "C" void calculate_input_trajectory(double *inp, double amplitude, double tsimul)
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
// modified trajectory by means of a cubic spline: -4·pi·t^3 + 6·pi·t^2 (qd[0] = 0 and qd[1] = 0 and q[0] = 0 and q[1] = 2*pi)
inp[njoint]=amplitude*sin((-4*M_PI*tsimul*tsimul*tsimul + 6*M_PI*tsimul*tsimul) + njoint*M_PI/4);
inp[njoint+NUM_JOINTS]=amplitude*12*M_PI*tsimul*(1-tsimul)*cos(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*njoint/4);
inp[njoint+2*NUM_JOINTS]=amplitude*(12*M_PI*(1-2*tsimul)*cos(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*njoint/4) + 144*M_PI*M_PI*tsimul*tsimul*(tsimul-1)*(tsimul-1)*sin(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*njoint/4));
}
}
extern "C" void calculate_input_error(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double centerneg[],double sigma[])
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{ centerpos[njoint]=trajectory_time*2.0/3;
centerneg[njoint]=trajectory_time*1.0/3;
sigma[njoint]=trajectory_time*0.1;//trajectory_time*0.1;
inp[njoint]=amplitude*exp(-(tsimul - centerpos[njoint])*(tsimul - centerpos[njoint])/(2*sigma[njoint]*sigma[njoint]))-amplitude*exp(-(tsimul - centerneg[njoint])*(tsimul - centerneg[njoint])/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+NUM_JOINTS]=amplitude*((2*centerpos[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-amplitude*((2*centerneg[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+2*NUM_JOINTS]=amplitude*(((2*centerpos[njoint]-2*tsimul)*(2*centerpos[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])))-amplitude*(((2*centerneg[njoint]-2*tsimul)*(2*centerneg[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
}
extern "C" void calculate_input_error_inverse(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double centerneg[],double sigma[])
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{ centerpos[njoint]=trajectory_time*2.0/3;
centerneg[njoint]=trajectory_time*1.0/3;
sigma[njoint]=trajectory_time*0.1;//trajectory_time*0.1;
inp[njoint]=-amplitude*exp(-(tsimul - centerpos[njoint])*(tsimul - centerpos[njoint])/(2*sigma[njoint]*sigma[njoint]))+amplitude*exp(-(tsimul - centerneg[njoint])*(tsimul - centerneg[njoint])/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+NUM_JOINTS]=-amplitude*((2*centerpos[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))+amplitude*((2*centerneg[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+2*NUM_JOINTS]=-amplitude*(((2*centerpos[njoint]-2*tsimul)*(2*centerpos[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])))+amplitude*(((2*centerneg[njoint]-2*tsimul)*(2*centerneg[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerneg[njoint]-tsimul)*(centerneg[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
}
extern "C" void calculate_input_error_pos(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double sigma[])
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{ centerpos[njoint]=trajectory_time*1.0/2.0;
sigma[njoint]=trajectory_time*0.1;//trajectory_time*0.1;
inp[njoint]=amplitude*exp(-(tsimul - centerpos[njoint])*(tsimul - centerpos[njoint])/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+NUM_JOINTS]=amplitude*((2*centerpos[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+2*NUM_JOINTS]=amplitude*(((2*centerpos[njoint]-2*tsimul)*(2*centerpos[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
}
extern "C" void calculate_input_error_pos_inverse(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double sigma[])
{
int njoint;
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{ centerpos[njoint]=trajectory_time*1.0/2.0;
sigma[njoint]=trajectory_time*0.1;//trajectory_time*0.1;
inp[njoint]=-amplitude*exp(-(tsimul - centerpos[njoint])*(tsimul - centerpos[njoint])/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+NUM_JOINTS]=-amplitude*((2*centerpos[njoint]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp[njoint+2*NUM_JOINTS]=-amplitude*(((2*centerpos[njoint]-2*tsimul)*(2*centerpos[njoint]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[njoint]-tsimul)*(centerpos[njoint]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
}
extern "C" void calculate_input_error_repetitions(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double centerneg[],double sigma[],int number_of_rep)
{
int njoint;
int nrepetition;
double center_pos_init;
double center_neg_init;
double factor;
double inp_aux=0.0;
double inp_aux_vel=0.0;
double inp_aux_acel=0.0;
factor=(trajectory_time/number_of_rep);
center_pos_init=factor*(2.0/3.0);
center_neg_init=factor*(1.0/3.0);
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
sigma[njoint]=factor*0.1;
for (nrepetition=0;nrepetition<number_of_rep;nrepetition++)
{
centerpos[(njoint*number_of_rep)+nrepetition]=center_pos_init+(trajectory_time/number_of_rep)*(nrepetition);
centerneg[(njoint*number_of_rep)+nrepetition]=center_neg_init+(trajectory_time/number_of_rep)*(nrepetition);
inp_aux+=(amplitude*exp(-(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])*(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint])))-(amplitude*exp(-(tsimul - centerneg[(njoint*number_of_rep)+nrepetition])*(tsimul - centerneg[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint])));
inp_aux_vel+=(amplitude*((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-amplitude*((2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
inp_aux_acel+=(amplitude*(((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])))-amplitude*(((2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))));
}
inp[njoint]=inp_aux;
inp[njoint+NUM_JOINTS]=inp_aux_vel;
inp[njoint+2*NUM_JOINTS]=inp_aux_acel;
inp_aux=0.0;
inp_aux_vel=0.0;
inp_aux_acel=0.0;
}
}
extern "C" void calculate_input_error_repetitions_inverse(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double centerneg[],double sigma[],int number_of_rep)
{
int njoint;
int nrepetition;
double center_pos_init;
double center_neg_init;
double factor;
double inp_aux=0.0;
double inp_aux_vel=0.0;
double inp_aux_acel=0.0;
factor=(trajectory_time/number_of_rep);
center_pos_init=factor*(2.0/3.0);
center_neg_init=factor*(1.0/3.0);
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
sigma[njoint]=factor*0.1;
for (nrepetition=0;nrepetition<number_of_rep;nrepetition++)
{
centerpos[(njoint*number_of_rep)+nrepetition]=center_pos_init+(trajectory_time/number_of_rep)*(nrepetition);
centerneg[(njoint*number_of_rep)+nrepetition]=center_neg_init+(trajectory_time/number_of_rep)*(nrepetition);
inp_aux+=-amplitude*exp(-(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])*(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint]))+amplitude*exp(-(tsimul - centerneg[(njoint*number_of_rep)+nrepetition])*(tsimul - centerneg[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint]));
inp_aux_vel+=-amplitude*((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))+amplitude*((2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp_aux_acel+=-amplitude*(((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])))+amplitude*(((2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerneg[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerneg[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
inp[njoint]=inp_aux;
inp[njoint+NUM_JOINTS]=inp_aux_vel;
inp[njoint+2*NUM_JOINTS]=inp_aux_acel;
inp_aux=0.0;
inp_aux_vel=0.0;
inp_aux_acel=0.0;
}
}
extern "C" void calculate_input_error_repetitions_pos(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double sigma[],int number_of_rep)
{
int njoint;
int nrepetition;
double center_pos_init;
double factor;
double inp_aux=0.0;
double inp_aux_vel=0.0;
double inp_aux_acel=0.0;
factor=(trajectory_time/number_of_rep);
center_pos_init=factor*(1.0/2.0);
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
sigma[njoint]=factor*0.1;
for (nrepetition=0;nrepetition<number_of_rep;nrepetition++)
{
centerpos[(njoint*number_of_rep)+nrepetition]=center_pos_init+(trajectory_time/number_of_rep)*(nrepetition);
inp_aux+=0.5+0.5*amplitude*exp(-(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])*(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint]));
inp_aux_vel+=0.5*amplitude*((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp_aux_acel+=0.5*amplitude*(((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
inp[njoint]=inp_aux;
inp[njoint+NUM_JOINTS]=inp_aux_vel;
inp[njoint+2*NUM_JOINTS]=inp_aux_acel;
inp_aux=0.0;
inp_aux_vel=0.0;
inp_aux_acel=0.0;
}
}
extern "C" void calculate_input_error_repetitions_pos_inverse(double *inp, double amplitude,double trajectory_time,double tsimul,double centerpos[],double sigma[],int number_of_rep)
{
int njoint;
int nrepetition;
double center_pos_init;
double factor;
double inp_aux=0.0;
double inp_aux_vel=0.0;
double inp_aux_acel=0.0;
factor=(trajectory_time/number_of_rep);
center_pos_init=factor*(1.0/2.0);
for(njoint=0;njoint<NUM_JOINTS;njoint++)
{
sigma[njoint]=factor*0.1;
for (nrepetition=0;nrepetition<number_of_rep;nrepetition++)
{
centerpos[(njoint*number_of_rep)+nrepetition]=center_pos_init+(trajectory_time/number_of_rep)*(nrepetition);
inp_aux+=-0.5-0.5*amplitude*exp(-(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])*(tsimul - centerpos[(njoint*number_of_rep)+nrepetition])/(2*sigma[njoint]*sigma[njoint]));
inp_aux_vel+=-0.5*amplitude*((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(2*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]));
inp_aux_acel+=-0.5*amplitude*(((2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)*(2*centerpos[(njoint*number_of_rep)+nrepetition]-2*tsimul)/(4*sigma[njoint]*sigma[njoint]*sigma[njoint]*sigma[njoint]))*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint]))-(1/sigma[njoint]*sigma[njoint])*exp(-(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)*(centerpos[(njoint*number_of_rep)+nrepetition]-tsimul)/(2*sigma[njoint]*sigma[njoint])));
}
inp[njoint]=inp_aux;
inp[njoint+NUM_JOINTS]=inp_aux_vel;
inp[njoint+2*NUM_JOINTS]=inp_aux_acel;
inp_aux=0.0;
inp_aux_vel=0.0;
inp_aux_acel=0.0;
}
}
//extern "C" void calculate_input_trajectory(double *inp, double amplitude, double tsimul)
/*
PURPOSE:
Inverse kinematic for Schunk robot(6DOF). x, y z position are given by geometric methods, yaw pich and roll are fixed as a constant.
Inverse kinematics (l1, l2 are the corresponding lenghts of the links)
q1=atan(y/x);
q3=acos((x^2+y^2+z^2-l1^2-l2^2)/(2*l1*l2));
q2=atan(z/sqrt(x^2+y^2))-atan((l2*sin(acos((x^2+y^2+z^2-l1^2-l2^2)/(2*l1*l2))))/(l1+l2*cos(acos((x^2+y^2+z^2-l1^2-l2^2)/(2*l1*l2)))));
The benchmark trajectory is an eight-like trajectory whose equations are given by:
cartesian coordinates
y1 = 0.1 * sin(2*pi *(-2*t^3+3*t^2) )+0.21502;
z1 = 0.1 * sin(4*pi *(-2*t^3+3*t^2) )+0.18502;
x1 =0.1;
the simulation time t(lineal) has been modified by means of a cubic spline so as to obtain a zero initial velocity and a zero final trajectory values
this is convenient for controlling the real robot.
cubic spline t=(-4*pi*t^3+6*pi*t^2) where (qd[0] = 0 and qd[1] = 0 and q[0] = 0 and q[1] = 2*pi)
this system can be rotated according to a certain angle.this is a rotation of the reference system (Y rotation)
CALLING SEQUENCE:
calculate_input_trajectory_shucnk ( *inputs, eight_plane_angle, tsimul);
INPUTS:
inputs buffer containing position, velocity and acceleration trajectories
eight_plane_angle angle in radian,the eight trajectory is located in a certain plane, this plane can be rotated along Y axis
tsimul simulation step
OUTPUT:
input buffer containing position, velocity and acceleration trajectories
*/
/*
{
//auxiliary Variables
double aux1A,aux1B,aux1C,auxK,auxL,auxM;
double rotsimul=0.0;
//POSITIONS
aux1A=(sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))/10.0 + 10751.0/50000.0);
aux1B=(cos(rotsimul)/10.0 + sin(rotsimul)*(sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))/10.0 + 9251.0/50000.0));
aux1C=(sin(rotsimul)/10.0 - cos(rotsimul)*(sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))/10.0 + 9251.0/50000.0));
auxK=cos(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul));
auxL=(6.0*tsimul - 6.0*tsimul*tsimul);
auxM=cos(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul));
//POSITIONS
inp[0]=atan(aux1A/aux1B);
inp[1]=-atan((61.0*sqrt(1.0 - (4000000.0*(aux1A*aux1A+ aux1B*aux1B+ aux1C*aux1C- 8621.0/40000.0)*(aux1A*aux1A+ aux1B*aux1B+ aux1C*aux1C- 8621.0/40000.0))/182329.0))/(200.0*((10*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0))) - atan(aux1C/sqrt(aux1A*aux1A+ aux1B*aux1B));
inp[2]=acos((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0);
//VELOCITIES
inp[3]=((M_PI*cos(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))*(- 6.0*tsimul*tsimul + 6.0*tsimul))/(5.0*(cos(rotsimul)/10.0 + sin(rotsimul)*(sin(4.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 9251.0/50000.0))) - (2.0*M_PI*cos(4.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))*sin(rotsimul)*(- 6.0*tsimul*tsimul + 6.0*tsimul)*(sin(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 10751.0/50000.0))/(5.0*aux1B*aux1B))/((sin(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 10751.0/50000.0)*(sin(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 10751.0/50000.0)/aux1B*aux1B + 1.0);
inp[4]= ((((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*aux1C)/(2.0*(aux1A*aux1A + aux1B*aux1B)*sqrt(aux1A*aux1A + aux1B*aux1B)) + (2.0*M_PI*cos(rotsimul)*auxM*auxL)/(5.0*sqrt(aux1A*aux1A + aux1B*aux1B)))/(aux1C*aux1C/(aux1A*aux1A + aux1B*aux1B) + 1.0) - ((61.0*sqrt(1.0 - (4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0))/(200.0*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)) + (20000.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/(2989.0*sqrt(1.0 - (4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)))/((3721.0*((4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0 - 1.0))/(40000.0*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)) - 1.0);
inp[5]=-((800.0*M_PI*cos(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))*(- 6.0*tsimul*tsimul + 6.0*tsimul)*(sin(2.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 10751.0/50000.0))/427.0 - (1600.0*M_PI*cos(rotsimul)*cos(4.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))*(- 6.0*tsimul*tsimul + 6.0*tsimul)*(sin(rotsimul)/10.0 - cos(rotsimul)*(sin(4.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 9251.0/50000.0)))/427.0 + (1600.0*M_PI*cos(4*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))*sin(rotsimul)*(- 6.0*tsimul*tsimul + 6.0*tsimul)*(cos(rotsimul)/10.0 + sin(rotsimul)*(sin(4.0*M_PI*(- 2.0*tsimul*tsimul*tsimul + 3.0*tsimul*tsimul))/10.0 + 9251.0/50000.0)))/427.0)/sqrt(- ((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0)*((2000.0*aux1A*aux1A)/427.0 + (2000*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0) + 1.0);
//ACCELERATIONS
inp[6]=-((M_PI*auxK*(12.0*tsimul - 6.0))/(5.0*aux1B) + (2.0*M_PI*M_PI*sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL)/(5.0*aux1B) - (2.0*M_PI*auxM*sin(rotsimul)*(12.0*tsimul - 6.0)*aux1A)/(5.0*aux1B*aux1B) - (8.0*M_PI*M_PI*sin(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1A)/(5.0*aux1B*aux1B) + (4.0*M_PI*M_PI*auxK*auxM*sin(rotsimul)*auxL*auxL)/(25.0*aux1B*aux1B) - (8.0*M_PI*M_PI*auxM*auxM*sin(rotsimul)*sin(rotsimul)*auxL*auxL*aux1A)/(25.0*aux1B*aux1B*aux1B))/(aux1A*aux1A/aux1B*aux1B + 1.0) - (((M_PI*auxK*auxL)/(5.0*aux1B) - (2.0*M_PI*auxM*sin(rotsimul)*auxL*aux1A)/(5.0*aux1B*aux1B))*((2.0*M_PI*auxK*auxL*aux1A)/(5.0*aux1B*aux1B) - (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1A*aux1A)/(5.0*aux1B*aux1B*aux1B)))/(aux1A*aux1A/aux1B*aux1B + 1.0)*(aux1A*aux1A/aux1B*aux1B + 1.0);
inp[7]= (((((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*aux1C*aux1C)/(aux1B*aux1B + aux1A*aux1A)*(aux1B*aux1B + aux1A*aux1A) + (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/(5.0*(aux1A*aux1A + aux1B*aux1B)))*((((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*aux1C)/(2.0*(aux1B*aux1B + aux1A*aux1A)*sqrt(aux1B*aux1B + aux1A*aux1A)) + (2.0*M_PI*cos(rotsimul)*auxM*auxL)/(5.0*sqrt(aux1B*aux1B + aux1A*aux1A))))/(aux1C*aux1C/(aux1B*aux1B + aux1A*aux1A) + 1.0)*(aux1C*aux1C/(aux1B*aux1B + aux1A*aux1A) + 1.0) - ((20000.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5))/(2989.0*sqrt(1 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)) - (61.0*sqrt(1 - (4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0))/(100.0*((10.0*aux1B*aux1B)/7.0 + (10*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) + (61.0*sqrt(1.0 - (4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0)*((4.0*M_PI*M_PI*auxK*auxK*auxL*auxL)/35.0 + (16.0*M_PI*M_PI*cos(rotsimul)*cos(rotsimul)*auxM*auxM*auxL*auxL)/35.0 + (16.0*M_PI*M_PI*auxM*auxM*sin(rotsimul)*sin(rotsimul)*auxL*auxL)/35.0 - (4.0*M_PI*auxK*(12.0*tsimul - 6.0)*aux1A)/7.0 - (8.0*M_PI*M_PI*sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1A)/7.0 + (8.0*M_PI*cos(rotsimul)*auxM*(12.0*tsimul - 6.0)*aux1C)/7.0 + (32.0*M_PI*M_PI*cos(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1C)/7.0 - (8.0*M_PI*auxM*sin(rotsimul)*(12.0*tsimul - 6.0)*aux1B)/7.0 - (32.0*M_PI*M_PI*sin(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1B)/7.0))/(200.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) + (20000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*((2.0*M_PI*M_PI*auxK*auxK*auxL*auxL)/25.0 + (8.0*M_PI*M_PI*cos(rotsimul)*cos(rotsimul)*auxM*auxM*auxL*auxL)/25.0 + (8.0*M_PI*M_PI*auxM*auxM*sin(rotsimul)*sin(rotsimul)*auxL*auxL)/25.0 - (2.0*M_PI*auxK*(12.0*tsimul - 6.0)*aux1A)/5.0 - (4.0*M_PI*M_PI*sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1A)/5.0 + (4.0*M_PI*cos(rotsimul)*auxM*(12.0*tsimul - 6.0)*aux1C)/5.0 + (16.0*M_PI*M_PI*cos(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1C)/5.0 - (4.0*M_PI*auxM*sin(rotsimul)*(12.0*tsimul - 6.0)*aux1B)/5.0 - (16.0*M_PI*M_PI*sin(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1B)/5.0))/(2989.0*sqrt(1.0 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0)) + (80000000000.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/(544981381.0*(1.0 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*sqrt(1.0 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7 + 1179.0/28000.0)) - (40000.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/(2989.0*sqrt(1.0 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)))/((3721.0*((4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0 - 1.0))/(40000.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) - 1.0) - ((3.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*aux1C)/(4.0*(aux1B*aux1B + aux1A*aux1A)*(aux1B*aux1B + aux1A*aux1A)*sqrt(aux1B*aux1B + aux1A*aux1A)) + (aux1C*((2.0*M_PI*auxK*(12.0*tsimul - 6.0)*aux1A)/5.0 - (8.0*M_PI*M_PI*auxM*auxM*sin(rotsimul)*sin(rotsimul)*auxL*auxL)/25.0 - (2.0*M_PI*M_PI*auxK*auxK*auxL*auxL)/25.0 + (4.0*M_PI*M_PI*sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*(12.0*tsimul - 6.0)*aux1B)/5.0 + (16.0*M_PI*M_PI*sin(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1B)/5.0))/(2.0*(aux1B*aux1B + aux1A*aux1A)*sqrt(aux1B*aux1B + aux1A*aux1A)) + (2.0*M_PI*cos(rotsimul)*auxM*(12.0*tsimul - 6.0))/(5.0*sqrt(aux1B*aux1B + aux1A*aux1A)) + (8.0*M_PI*M_PI*cos(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL)/(5.0*sqrt(aux1B*aux1B + aux1A*aux1A)) + (2.0*M_PI*cos(rotsimul)*auxM*((2.0*M_PI*auxK*auxL*aux1A)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*auxL)/(5.0*(aux1B*aux1B + aux1A*aux1A)*sqrt(aux1B*aux1B + aux1A*aux1A)))/(aux1C*aux1C/(aux1A*aux1A + aux1B*aux1B) + 1.0) + (((200.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/(49.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) - (3721.0*((4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0 - 1.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0))/(20000.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)))*((61.0*sqrt(1.0 - (4000000.0*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/182329.0)*((4.0*M_PI*auxK*auxL*aux1A)/7.0 - (8.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/7.0 + (8.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/7.0))/(200.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) + (20000.0*((2.0*M_PI*auxK*auxL*aux1A)/5.0 - (4.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/5.0 + (4.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/5.0)*(aux1A*aux1A + aux1B*aux1B + aux1C*aux1C - 8621.0/40000.0))/(2989.0*sqrt(1.0 - (4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0)*((10.0*aux1A*aux1A)/7.0 + (10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + 1179.0/28000.0))))/((3721.0*((4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0 - 1.0))/(40000.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) - 1.0)*((3721.0*((4000000.0*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0)*(aux1B*aux1B + aux1C*aux1C + aux1A*aux1A - 8621.0/40000.0))/182329.0 - 1.0))/(40000.0*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)*((10.0*aux1B*aux1B)/7.0 + (10.0*aux1C*aux1C)/7.0 + (10.0*aux1A*aux1A)/7.0 + 1179.0/28000.0)) - 1.0);
inp[8]=-((160.0*M_PI*M_PI*auxK*auxK*auxL*auxL)/427.0 + (640.0*M_PI*M_PI*cos(rotsimul)*cos(rotsimul)*auxM*auxM*auxL*auxL)/427.0 + (640.0*M_PI*M_PI*auxM*auxM*sin(rotsimul)*sin(rotsimul)*auxL*auxL)/427.0 - (800.0*M_PI*auxK*(12.0*tsimul - 6.0)*aux1A)/427.0 - (1600.0*M_PI*M_PI*sin(2.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1A)/427.0 + (1600.0*M_PI*cos(rotsimul)*auxM*(12.0*tsimul - 6.0)*aux1C)/427.0 + (6400.0*M_PI*M_PI*cos(rotsimul)*sin(4*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1C)/427.0 - (1600.0*M_PI*auxM*sin(rotsimul)*(12.0*tsimul - 6.0)*aux1B)/427.0 - (6400.0*M_PI*M_PI*sin(rotsimul)*sin(4.0*M_PI*(3.0*tsimul*tsimul - 2.0*tsimul*tsimul*tsimul))*auxL*auxL*aux1B)/427.0)/sqrt(1.0 - ((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0)*((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0)) - (((800.0*M_PI*auxK*auxL*aux1A)/427.0 - (1600.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/427.0 + (1600.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/427.0)*((800.0*M_PI*auxK*auxL*aux1A)/427.0 - (1600.0*M_PI*cos(rotsimul)*auxM*auxL*aux1C)/427.0 + (1600.0*M_PI*auxM*sin(rotsimul)*auxL*aux1B)/427.0)*((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0))/(1.0 - ((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0)*((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0))*sqrt(1.0 - ((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0)*((2000.0*aux1A*aux1A)/427.0 + (2000.0*aux1B*aux1B)/427.0 + (2000.0*aux1C*aux1C)/427.0 - 8621.0/8540.0));
}
*/
extern "C" void calculate_input_trajectory_max_amplitude(double trajectory_time, double amplitude, double *min_traj_amplitude, double *max_traj_amplitude)
{
double inp[NUM_JOINTS*3],traj_val;
double tsimul;
int njoint,nmagnit;
for(nmagnit=0;nmagnit<3;nmagnit++)
{
min_traj_amplitude[nmagnit]=-log(0.0L); // +Infinite
max_traj_amplitude[nmagnit]=log(0.0L); // -Infinite
}
for(tsimul=0.0;tsimul<trajectory_time;tsimul+=SIM_SLOT_LENGTH)
{
calculate_input_trajectory(inp,amplitude,tsimul);
for(njoint=0;njoint<NUM_JOINTS;njoint++)
for(nmagnit=0;nmagnit<3;nmagnit++)
{
traj_val=inp[njoint+nmagnit*NUM_JOINTS];
if(traj_val > max_traj_amplitude[nmagnit])
max_traj_amplitude[nmagnit]=traj_val;
if(traj_val < min_traj_amplitude[nmagnit])
min_traj_amplitude[nmagnit]=traj_val;
}
}
}
double calculate_RBF_width(double rbf_distance, double overlap)
{
double half_rbf_pos_inc=rbf_distance/2;
return(sqrt(half_rbf_pos_inc*half_rbf_pos_inc/(-2*log(overlap))));
}
extern "C" void printRBFs(struct rbf_set *rbfs)
{
int nneu;
double rbf_cur_pos,rbf_pos_inc,rbf_width;
rbf_cur_pos=rbfs->first_bell_pos;
rbf_pos_inc=(rbfs->last_bell_pos - rbfs->first_bell_pos)/(rbfs->num_rbfs-1);
rbf_width=calculate_RBF_width(rbf_pos_inc,rbfs->bell_overlap);
printf("[");
for(nneu=0;nneu<rbfs->num_rbfs;nneu++,rbf_cur_pos+=rbf_pos_inc)
printf("%g*exp(-(x-%g)^2/(2*%g^2));",rbfs->bell_amp, rbf_cur_pos, rbf_width);
printf("0]\n");
}
void generate_activityRBF(Simulation *sim, double cur_slot_time, struct rbf_set *rbfs, double input_var, long first_input_neuron, double *last_spk_times, double max_spk_freq)
{
int nneu;
double rbf_cur_pos,rbf_pos_inc,neu_i_current,rbf_width;
double spk_per,cur_spk_time;
Neuron *cur_neuron;
rbf_cur_pos=rbfs->first_bell_pos;
rbf_pos_inc=(rbfs->last_bell_pos - rbfs->first_bell_pos)/(rbfs->num_rbfs-1);
rbf_width=calculate_RBF_width(rbf_pos_inc,0.2);
for(nneu=0;nneu<rbfs->num_rbfs;nneu++,rbf_cur_pos+=rbf_pos_inc)
{
neu_i_current=gaussian_function(rbfs->bell_amp, rbf_cur_pos, rbf_width, input_var);
spk_per=1/(max_spk_freq*neu_i_current);
cur_neuron=sim->GetNetwork()->GetNeuronAt(first_input_neuron+nneu);
cur_spk_time=last_spk_times[nneu]+spk_per;
if(cur_spk_time<cur_slot_time)
cur_spk_time=cur_slot_time;
for(;cur_spk_time<cur_slot_time+SIM_SLOT_LENGTH;cur_spk_time+=spk_per)
{
sim->GetQueue()->InsertInputSpikeEvent(cur_spk_time, cur_neuron);
last_spk_times[nneu]=cur_spk_time;
}
}
}
extern "C" void generate_input_traj_activity(Simulation *neural_sim, double cur_slot_time, double *input_vars, double *min_traj_amplitude, double *max_traj_amplitude)
{
static double last_spk_times[NUM_TRAJECTORY_INPUT_NEURONS]={0.0};
double max_spk_freq=100.0;
long first_neuron_of_var,num_first_neuron_in_input;
struct rbf_set rbfs_pos,rbfs_vel; //,rbfs_acc;
int nvar;
rbfs_pos.bell_amp=1.0;
rbfs_pos.bell_overlap=0.2;
rbfs_pos.first_bell_pos=min_traj_amplitude[0];
rbfs_pos.last_bell_pos=max_traj_amplitude[0];
rbfs_pos.num_rbfs=NUM_RBFS;
rbfs_vel.bell_amp=1.0;
rbfs_vel.bell_overlap=0.2;
rbfs_vel.first_bell_pos=min_traj_amplitude[1];
rbfs_vel.last_bell_pos=max_traj_amplitude[1];
rbfs_vel.num_rbfs=NUM_RBFS;
/*
rbfs_acc.bell_amp=1.0;
rbfs_acc.bell_overlap=0.2;
rbfs_acc.first_bell_pos=-traj_amplitude*4*M_PI*M_PI;
rbfs_acc.last_bell_pos=traj_amplitude*4*M_PI*M_PI;
rbfs_acc.num_rbfs=NUM_RBFS;
*/
// Position of joints
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=NUM_RBFS*nvar;
first_neuron_of_var=FIRST_INPUT_NEURON+num_first_neuron_in_input;
generate_activityRBF(neural_sim,cur_slot_time,&rbfs_pos,input_vars[nvar],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
// Velocity of joints
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=(NUM_RBFS*NUM_JOINTS)*1 + NUM_RBFS*nvar;
first_neuron_of_var=FIRST_INPUT_NEURON+num_first_neuron_in_input;
generate_activityRBF(neural_sim,cur_slot_time,&rbfs_vel,input_vars[nvar+NUM_JOINTS],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
// Acceleration of joints
/*
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=(NUM_RBFS*NUM_JOINTS)*2 + NUM_RBFS*nvar;
first_neuron_of_var=FIRST_INPUT_NEURON+num_first_neuron_in_input;
generate_activityRBF(sim,cur_slot_time,&rbfs_acc,input_vars[nvar+NUM_JOINTS*2],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
*/
}
extern "C" void generate_robot_state_activity(Simulation *neural_sim, double cur_slot_time, double *robot_state_vars, double *min_traj_amplitude, double *max_traj_amplitude)
{
static double last_spk_times[NUM_ROBOT_STATE_INPUT_NEURONS]={0.0};
double max_spk_freq=100.0;
long first_neuron_of_var,num_first_neuron_in_input,first_robot_state_neuron;
struct rbf_set rbfs_pos,rbfs_vel; //,rbfs_acc;
int nvar;
rbfs_pos.bell_amp=1.0;
rbfs_pos.bell_overlap=0.2;
rbfs_pos.first_bell_pos=min_traj_amplitude[0];
rbfs_pos.last_bell_pos=max_traj_amplitude[0];
rbfs_pos.num_rbfs=NUM_RBFS;
rbfs_vel.bell_amp=1.0;
rbfs_vel.bell_overlap=0.2;
rbfs_vel.first_bell_pos=min_traj_amplitude[1];
rbfs_vel.last_bell_pos=max_traj_amplitude[1];
rbfs_vel.num_rbfs=NUM_RBFS;
/*
rbfs_acc.bell_amp=1.0;
rbfs_acc.bell_overlap=0.2;
rbfs_acc.first_bell_pos=-traj_amplitude*4*M_PI*M_PI;
rbfs_acc.last_bell_pos=traj_amplitude*4*M_PI*M_PI;
rbfs_acc.num_rbfs=NUM_RBFS;
*/
first_robot_state_neuron=FIRST_INPUT_NEURON+(NUM_RBFS*NUM_JOINTS)*2; // The first input neuron which encodes the robot's state is the following to the last input trajectory neuron
// Position of joints
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=NUM_RBFS*nvar;
first_neuron_of_var=first_robot_state_neuron+num_first_neuron_in_input;
generate_activityRBF(neural_sim,cur_slot_time,&rbfs_pos,robot_state_vars[nvar],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
// Velocity of joints
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=(NUM_RBFS*NUM_JOINTS)*1 + NUM_RBFS*nvar;
first_neuron_of_var=first_robot_state_neuron+num_first_neuron_in_input;
generate_activityRBF(neural_sim,cur_slot_time,&rbfs_vel,robot_state_vars[nvar+NUM_JOINTS],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
// Acceleration of joints
/*
for(nvar=0;nvar<NUM_JOINTS;nvar++)
{
num_first_neuron_in_input=(NUM_RBFS*NUM_JOINTS)*2 + NUM_RBFS*nvar;
first_neuron_of_var=FIRST_INPUT_NEURON+num_first_neuron_in_input;
generate_activityRBF(sim,cur_slot_time,&rbfs_acc,robot_state_vars[nvar+NUM_JOINTS*2],first_neuron_of_var,last_spk_times+num_first_neuron_in_input,max_spk_freq);
}
*/
}
/////////////////////// GENERATE LEARNING ACTIVITY ///////////////////
double compute_PD_error(double desired_position, double desired_velocity, double actual_position, double actual_velocity, double kp, double kd)
{
double t_error;
t_error=kp*(actual_position-desired_position) + kd*(actual_velocity-desired_velocity);
return(t_error);
}
double error_sigmoid(double error_torque, double max_error_torque)
{
return(0.15 + 0.85/(1+exp(-10*error_torque/max_error_torque+4)));
//return(0.15 + 0.8/(1+exp(-10*error_torque/max_error_torque+4)))
}
void generate_stochastic_activity(Simulation *sim, double cur_slot_init, double input_current, long first_learning_neuron, long num_learning_neurons, double *last_spk_times, double max_spk_freq)
{
double cur_slot_end,min_spk_per,cur_spk_init_zone,cur_spk_end_zone;
int nneu,spk,max_spk_amplitude,medium_spk_amplitude_up,medium_spk_amplitude_down,min_spk_amplitude,num_spk;
double current_freq_factor=1.1;// 1.1 original FIJATEEEEEEEEEEEEE AQUIIIIIIIIIIIIIIIIIIIIIIIIIIII
// Generating Bursting, number of spike generated according to the received error.
max_spk_amplitude=4;
medium_spk_amplitude_up=3;
medium_spk_amplitude_down=2;
min_spk_amplitude=1;
if(input_current>=0.95)
num_spk= max_spk_amplitude;
else if((input_current <0.95) &&(input_current >0.85) )
num_spk= medium_spk_amplitude_up;
else if((input_current <=0.85) &&(input_current >0.75) )
num_spk= medium_spk_amplitude_down;
else if((input_current <=0.75) &&(input_current >0.5) )
num_spk= min_spk_amplitude;
else
num_spk= 1;
Neuron *cur_neuron;
min_spk_per=1/max_spk_freq;
cur_slot_end=cur_slot_init+SIM_SLOT_LENGTH*(double) num_spk;
for(nneu=0;nneu<num_learning_neurons;nneu++)
{
cur_spk_init_zone=last_spk_times[nneu]+min_spk_per;
if(cur_spk_init_zone<cur_slot_init)
cur_spk_init_zone=cur_slot_init;
for(;cur_spk_init_zone<cur_slot_end;cur_spk_init_zone+=min_spk_per)
{
cur_spk_end_zone=cur_spk_init_zone+min_spk_per;
if(cur_spk_end_zone > cur_slot_end)
cur_spk_end_zone=cur_slot_end;
if((cur_spk_end_zone-cur_spk_init_zone)*input_current*current_freq_factor*max_spk_freq > rand()/(double)RAND_MAX)
{
cur_neuron=sim->GetNetwork()->GetNeuronAt(first_learning_neuron+nneu);
double cur_spk_time=((cur_slot_init+SIM_SLOT_LENGTH+cur_spk_init_zone)/2.0);//MIRA AQUÍ MAÑANA
for(spk=0;spk<num_spk;spk++){
sim->GetQueue()->InsertInputSpikeEvent(cur_spk_time, cur_neuron);
cur_spk_time+=SIM_SLOT_LENGTH;
}
//sim->GetQueue()->InsertInputSpikeEvent(cur_spk_time, cur_neuron);
last_spk_times[nneu]=cur_spk_time;
}
}
}
}
extern "C" void calculate_error_signals(double *input_vars, double *state_vars, double *error_vars)
{
int num_joint;
for(num_joint=0;num_joint<NUM_JOINTS;num_joint++)
{
double desired_position, desired_velocity, actual_position, actual_velocity;
desired_position=input_vars[num_joint];
desired_velocity=input_vars[NUM_JOINTS+num_joint];
actual_position=state_vars[num_joint];
actual_velocity=state_vars[NUM_JOINTS+num_joint];
error_vars[num_joint]=compute_PD_error(desired_position, desired_velocity, actual_position, actual_velocity, ROBOT_JOINT_ERROR_KP[num_joint], ROBOT_JOINT_ERROR_KD[num_joint]);
}
}
extern "C" void calculate_learning_signals(double *error_vars, double *output_vars, double *learning_vars)
{
int num_joint;
double torque_error;
double i_neu_posit=0.0;
double i_neu_negat=0.0;
const double i_base=0.0; // 0.15;
const double max_low_torque_factor=0.2;
for(num_joint=0;num_joint<NUM_JOINTS;num_joint++)
{
double max_torque, corrective_torque_posit, corrective_torque_negat;
torque_error=error_vars[num_joint];
max_torque=MAX_ROBOT_JOINT_TORQUE[num_joint];
corrective_torque_posit=output_vars[num_joint*2];
corrective_torque_negat=output_vars[num_joint*2+1];
// use a sigmoid to adapt torque error signals
if(torque_error<=0.0)
{
i_neu_posit = error_sigmoid(-torque_error,max_torque);
if(corrective_torque_negat > max_torque*max_low_torque_factor)
i_neu_negat=0.0;
else
i_neu_negat=i_base;
}
else
if(torque_error>0.0)
{
i_neu_negat = error_sigmoid(torque_error,max_torque);
if(corrective_torque_posit > max_torque*max_low_torque_factor)
i_neu_posit=0.0;
else
i_neu_posit=i_base;
}
learning_vars[num_joint*2]=i_neu_posit;
learning_vars[num_joint*2+1]=i_neu_negat;
}
}
extern "C" void generate_learning_activity(Simulation *neural_sim, double cur_slot_init, double *learning_vars)
{
static double last_spk_times[NUM_LEARNING_NEURONS]={0.0};
int num_joint;
const double max_spk_freq=25; // maximum learning-neuron firing frequency
for(num_joint=0;num_joint<NUM_JOINTS;num_joint++)
{
long first_learning_neuron, first_neuron_in_learning_neurons;
double i_neu_posit, i_neu_negat;
i_neu_posit=learning_vars[num_joint*2];
i_neu_negat=learning_vars[num_joint*2+1];
first_neuron_in_learning_neurons=(NUM_LEARNING_NEURONS_PER_OUTPUT_VAR*2)*num_joint;
first_learning_neuron=FIRST_LEARNING_NEURON+first_neuron_in_learning_neurons;
generate_stochastic_activity(neural_sim, cur_slot_init, i_neu_posit, first_learning_neuron, NUM_LEARNING_NEURONS_PER_OUTPUT_VAR, last_spk_times+first_neuron_in_learning_neurons, max_spk_freq);
generate_stochastic_activity(neural_sim, cur_slot_init, i_neu_negat, first_learning_neuron+NUM_LEARNING_NEURONS_PER_OUTPUT_VAR, NUM_LEARNING_NEURONS_PER_OUTPUT_VAR, last_spk_times+first_neuron_in_learning_neurons+NUM_LEARNING_NEURONS_PER_OUTPUT_VAR, max_spk_freq);
}
}
//////////////////////////// GENERATE OUTPUT /////////////////////////
extern "C" int compute_output_activity(Simulation *neural_sim, double *output_vars)
{
long nspks, spkneu;
double spktime;
static double current_time_check=0;
double max_time_check=current_time_check;
int noutputvar;
ArrayOutputSpikeDriver *neural_activity_output_array;
const double tau_time_constant=0.060; // 20ms
const double kernel_amplitude=sqrt(0.0000175/tau_time_constant); // normalization coefficient
//const double kernel_amplitude=sqrt(0.00000275/tau_time_constant); // normalization coefficient
// Update value of output vars to the current time
for(noutputvar=0;noutputvar<NUM_OUTPUT_VARS;noutputvar++)
output_vars[noutputvar]*=exp(-SIM_SLOT_LENGTH/tau_time_constant);
nspks=0;
neural_activity_output_array=(ArrayOutputSpikeDriver *)neural_sim->GetOutputSpikeDriver(0); // The first output spike driver in the list is the ArrayOutputSpikeDriver
while(neural_activity_output_array->RemoveBufferedSpike(spktime,spkneu))
{
if(spktime < current_time_check){
printf("Unexpected spike time:%lf of neuron: %li (current simulation time: %f)\n",spktime,spkneu,current_time_check);
}
if(spktime>max_time_check){
max_time_check=spktime;
}
if(spkneu >= FIRST_OUTPUT_NEURON && spkneu < FIRST_OUTPUT_NEURON+NUM_OUTPUT_NEURONS){
noutputvar=(spkneu-FIRST_OUTPUT_NEURON)/NUM_NEURONS_PER_OUTPUT_VAR;
output_vars[noutputvar]+=kernel_amplitude;
}
nspks++;
}
current_time_check=max_time_check;
neural_activity_output_array->OutputBuffer.clear();
return(nspks);
}
///////////////////////////// VARIABLES LOG //////////////////////////
extern "C" int create_log(struct log *log, int total_traj_executions, int trajectory_time)
{
//Total number of registers that will be stored in the computer memory during the simulation
int n_log_regs=total_traj_executions*(int)((trajectory_time-((SIM_SLOT_LENGTH)/2.0))/(float)(SIM_SLOT_LENGTH) + 1);
int errorn;
log->nregs=0;
log->regs=(struct log_reg *)malloc(sizeof(struct log_reg)*n_log_regs);
if(log)
errorn=0;
else
errorn=-1;
return(errorn);
}
extern "C" void log_vars(struct log *log, double time, double *input_vars, double *state_vars, double *torque_vars, double *output_vars, double *learning_vars, double *error_vars, float elapsed_time, unsigned long spk_counter)
{
int nvar;
log->regs[log->nregs].time=(float)time;
log->regs[log->nregs].spk_counter=spk_counter;
log->regs[log->nregs].consumed_time=elapsed_time;
for(nvar=0;nvar<NUM_JOINTS*3;nvar++)
log->regs[log->nregs].cereb_input_vars[nvar]=input_vars?(float)input_vars[nvar]:0;
for(nvar=0;nvar<NUM_JOINTS*3;nvar++)
log->regs[log->nregs].robot_state_vars[nvar]=state_vars?(float)state_vars[nvar]:0;
for(nvar=0;nvar<NUM_JOINTS;nvar++)
log->regs[log->nregs].robot_torque_vars[nvar]=torque_vars?(float)torque_vars[nvar]:0;
for(nvar=0;nvar<NUM_OUTPUT_VARS;nvar++)
log->regs[log->nregs].cereb_output_vars[nvar]=output_vars?(float)output_vars[nvar]:0;
for(nvar=0;nvar<NUM_OUTPUT_VARS;nvar++)
log->regs[log->nregs].cereb_learning_vars[nvar]=learning_vars?(float)learning_vars[nvar]:0;
for(nvar=0;nvar<NUM_JOINTS;nvar++)
log->regs[log->nregs].robot_error_vars[nvar]=error_vars?(float)error_vars[nvar]:0;
log->nregs++;
}
extern "C" void log_vars_reduced(struct log *log, double time, double *input_vars,double *output_vars_normalized,double *output_vars, double *learning_vars, double *error_vars, float elapsed_time, unsigned long spk_counter)
{
int nvar;
log->regs[log->nregs].time=(float)time;
log->regs[log->nregs].spk_counter=spk_counter;
log->regs[log->nregs].consumed_time=elapsed_time;
for(nvar=0;nvar<NUM_JOINTS*3;nvar++)
log->regs[log->nregs].cereb_input_vars[nvar]=input_vars?(float)input_vars[nvar]:0;
for(nvar=0;nvar<NUM_JOINTS;nvar++)
log->regs[log->nregs].robot_state_vars[nvar]=output_vars?(float)output_vars_normalized[2*nvar]-(float)output_vars_normalized[2*(nvar)+1]:0;
for(nvar=0;nvar<NUM_JOINTS;nvar++)
log->regs[log->nregs].robot_torque_vars[nvar]=output_vars?(float)output_vars[2*nvar]-(float)output_vars[2*(nvar)+1]:0;
for(nvar=0;nvar<NUM_OUTPUT_VARS;nvar++)
log->regs[log->nregs].cereb_output_vars[nvar]=output_vars?(float)output_vars[nvar]:0;
for(nvar=0;nvar<NUM_OUTPUT_VARS;nvar++)
log->regs[log->nregs].cereb_learning_vars[nvar]=learning_vars?(float)learning_vars[nvar]:0;
for(nvar=0;nvar<NUM_JOINTS;nvar++)
log->regs[log->nregs].robot_error_vars[nvar]=error_vars?(float)error_vars[nvar]:0;
log->nregs++;
}
extern "C" int save_and_finish_log(struct log *log, char *file_name)
{
int ret;
int cur_reg,cur_var;
FILE *fd;
fd=fopen(file_name,"wt");
if(fd)
{
time_t current_time;
char *cur_time_string;
ret=0;
fprintf(fd,"%s Log file generated by save_log()",COMMENT_CHARS);
// Get current time
current_time = time(NULL);
if(current_time != ((time_t)-1))
{
cur_time_string = ctime(¤t_time);
if(cur_time_string)
fprintf(fd," on %s",cur_time_string);
}
fprintf(fd,"\n%s Numer of registers: %i. Columns per register: time consumed_time spk_counter %i_input_vars %i_state_vars %i_torque_vars %i_output_vars %i_learning_vars %i_error_vars\n\n",COMMENT_CHARS,log->nregs,NUM_JOINTS*3,NUM_JOINTS*3,NUM_JOINTS,NUM_OUTPUT_VARS,NUM_OUTPUT_VARS,NUM_JOINTS);
for(cur_reg=0;cur_reg<log->nregs;cur_reg++)
{
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].time);
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].consumed_time);
ret|=!fprintf(fd,"%lu ",log->regs[cur_reg].spk_counter);
for(cur_var=0;cur_var<NUM_JOINTS*3;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].cereb_input_vars[cur_var]);
for(cur_var=0;cur_var<NUM_JOINTS*3;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].robot_state_vars[cur_var]);
for(cur_var=0;cur_var<NUM_JOINTS;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].robot_torque_vars[cur_var]);
for(cur_var=0;cur_var<NUM_OUTPUT_VARS;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].cereb_output_vars[cur_var]);
for(cur_var=0;cur_var<NUM_OUTPUT_VARS;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].cereb_learning_vars[cur_var]);
for(cur_var=0;cur_var<NUM_JOINTS;cur_var++)
ret|=!fprintf(fd,"%10.10g ",log->regs[cur_reg].robot_error_vars[cur_var]);
fprintf(fd,"\n");
}
fclose(fd);
free(log->regs);
}
else
ret=1;
return(ret);
}
extern "C" void calculate_input_activity_for_one_trajectory(Simulation *neural_sim, double time){
int i=0;
int j=0;
int n=100;
for(double t=0; t<0.999; t+=0.002){
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(n));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(n+1));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(n+2));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(n+3));
n+=4;
if(i<19){
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(j));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(j+1));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(j+2));
neural_sim->GetQueue()->InsertInputSpikeEvent(time+t, neural_sim->GetNetwork()->GetNeuronAt(j+3));
}
i++;
if(i==20){
i=0;
j+=4;
}
}
}
extern "C" void init_real_time_restriction(Simulation * neural_sim, float slot_time, float max_simulation_delay, float first_section, float second_section, float third_section){
neural_sim->RealTimeRestrictionObject->SetParameterWatchDog(slot_time, max_simulation_delay, first_section, second_section,third_section);
}
extern "C" void start_real_time_restriction(Simulation * neural_sim){
neural_sim->RealTimeRestrictionObject->Watchdog();
}
extern "C" void reset_real_time_restriction(Simulation * neural_sim){
neural_sim->RealTimeRestrictionObject->ResetWatchDog();
}
extern "C" void next_step_real_time_restriction(Simulation * neural_sim){
neural_sim->RealTimeRestrictionObject->NextStepWatchDog();
}
extern "C" void stop_real_time_restriction(Simulation * neural_sim){
neural_sim->RealTimeRestrictionObject->StopWatchDog();
}
