/*******************************************************************************
* SimulatedRobotControl3laws.c *
* ----------------------- *
* copyright : (C) 2013 by Richard R. Carrillo and Niceto R. Luque *
* email : rcarrillo, nluque 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 SimulatedRobotControl.c
*
* \author Niceto R. Luque
* \author Richard R. Carrillo
* \date 7 of November 2013
* In this file the main robot-control loop is implemented.
*/
#include <iostream>
using namespace std;
#if defined(_DEBUG) && (defined(_WIN32) || defined(_WIN64))
# define _CRTDBG_MAP_ALLOC
# include <crtdbg.h> // To check for memory leaks
#endif
#if defined(__APPLE_CC__)
// MAC OS X includes
# define REAL_TIME_OSX
#elif defined (__linux__)
// Linux includes
# define REAL_TIME_LINUX
#elif (defined(_WIN32) || defined(_WIN64))
# define REAL_TIME_WINNT
#else
# error "Unsupported platform"
#endif
#if defined(REAL_TIME_OSX)
# include <mach/mach.h>
# include <mach/mach_time.h>
# include <CoreServices/CoreServices.h>
# include <unistd.h>
#elif defined(REAL_TIME_LINUX)
# include <time.h>
#elif defined(REAL_TIME_WINNT)
# include <windows.h>
#endif
#include <stdio.h>
#include "../include/interface/C_interface_for_robot_control.h"
//ROBOT//
#include "../include/arm_robot_simulator/ArmRobotSimulation.h"
#include "../include/openmp/openmp.h"
// Neural-network simulation files
#define NET_FILE "NetWithGolgiStep3_1.net"//"NetDistributedPlasticity2dcnCos.net"// Neural-network definition file used by EDLUT
#define INPUT_WEIGHT_FILE "WeightWithGolgiStep3_1.net"//"WeightNetDistributedPlasticity2dcnFinal.net"// Neural-network input weight file used by EDLUT
#define OUTPUT_WEIGHT_FILE "OutputWeight.dat" // Neural-network output weight file used by EDLUT
#define WEIGHT_SAVE_PERIOD 0 // The weights will be saved each period (in seconds) (0=weights are not saved periodically)
//#define INPUT_ACTIVITY_FILE "ActivityClosingLoop2DCNGR25trajectory1seg_during5000seg.dat"//"Activity2dcnALLLearnings25trajectory1seg_during2500seg.dat" //Optional input activity file
#define INPUT_ACTIVITY_FILE 0
#define OUTPUT_ACTIVITY_FILE "OutputActivity.dat" // Output activity file used to register the neural network activity
#define LOG_FILE "vars.dat" // Log file used to register the simulation variables
#define REAL_TIME_NEURAL_SIM 0 // EDLUT's simulation mode (0=No real-time neural network simulation 1=For real robot control)
#define FIRST_REAL_TIME_RESTRICTION 0.7
#define SECOND_REAL_TIME_RESTRICTION 0.8
#define THIRD_REAL_TIME_RESTRICTION 0.9
#define MAX_SIMULATION_DELAY 0.05f
#define NUMBER_OF_OPENMP_QUEUES 1
#define NUMBER_OF_OPENMP_THREADS 1
// external error
//#define ERROR_AMP 1 // Amplitude of the injected error
//#define NUM_REP 1 // Number of repetition of the exponential shape along the Trajectory Time
//#define ROBOT_JOINT_ERROR_NORM 160 // proportional error {160}
#define TRAJ_POS_AMP 0.1 // Amplitude of the desired robot's trajectory
#define TRAJECTORY_TIME 1 // Simulation time in seconds required to execute the desired trajectory once
#define MAX_TRAJ_EXECUTIONS 10 // Maximum number of trajectories repetitions that will be executed by the robot
#define ERROR_DELAY_TIME 0.1 // Delay after calculating the error vars
// Robot's dynamics files
#define ROBOT_VAR_FILE_NAME "MANIPULATORS.mat" // MATLAB's file containing the robot's specifications.
#define ROBOT_BASE_VAR_NAME "RRedKuKa" // Variable name in ROBOT_VAR_FILE_NAME which contains the base robot specifications.
#define ROBOT_SIMUL_VAR_NAME "RRedKuKadet6" // Variable name in ROBOT_VAR_FILE_NAME which contains the simulated robot specifications.
const double ROBOT_GRAVITY[3]={0, 0, 9.81}; // Earth's standard acceleration due to gravity [Gx Gy Gz]
const double ROBOT_EXTERNAL_FORCE[6]={0, 0, 0, 0, 0, 0}; // External force on manipulator tip [Fx Fy Fz MOMENTUMx MOMENTUMy MOMENTUMz]
///////////////////////////// MAIN LOOP //////////////////////////
int main(int ac, char *av[])
{
int errorn;
long total_output_spks;
double input_traject_vars[NUM_JOINTS*3]; // Desired trajectory position, velocity and acceletarion
double robot_state_vars[NUM_JOINTS*3]; // Actual robot position, velocity and acceleration
double robot_inv_dyn_torque[NUM_JOINTS]; // Robot's inverse dynamics torque
double total_torque[NUM_JOINTS]; // Total torque applied to the robot
double robot_error_vars[NUM_JOINTS]; // Joint error (PD correction)
double cerebellar_output_vars[NUM_OUTPUT_VARS]={0.0}; // Corrective cerebellar output torque
double cerebellar_error_vars[NUM_JOINTS*3]={0.0}; // error corrective cerebellar output torque
double cerebellar_learning_vars[NUM_OUTPUT_VARS]={0.0}; // Error-related learning signals
// delayed Error-related learning signals
double *delayed_cerebellar_learning_vars;
// Robot's dynamics variables
mxArray *robot_inv_dyn_object=NULL, *robot_dir_dyn_object=NULL;
struct integrator_buffers num_integration_buffers; // Integration buffers used to simulate the robot
// Simul variables
Simulation *neural_sim;
// Robot variables
int n_robot_joints;
// Time variables
double sim_time,cur_traject_time;
float slot_elapsed_time,sim_elapsed_time;
int n_traj_exec;
// Delays
struct delay cerebellar_delay;
struct delay cerebellar_learning_delay;
struct delay cerebellar_delay_normalized;
// Variable for logging the simulation state variables
struct log var_log;
#if defined(REAL_TIME_WINNT)
// Variables for consumed-CPU-time measurement
LARGE_INTEGER startt,endt,freq;
#elif defined(REAL_TIME_OSX)
uint64_t startt, endt, elapsed;
static mach_timebase_info_data_t freq;
#elif defined(REAL_TIME_LINUX)
// Calculate time taken by a request - Link with real-time library -lrt
struct timespec startt, endt, freq;
#endif
#if defined(_DEBUG) && (defined(_WIN32) || defined(_WIN64))
// _CrtMemState state0;
_CrtSetReportMode(_CRT_WARN, _CRTDBG_MODE_FILE);
_CrtSetReportFile(_CRT_WARN, _CRTDBG_FILE_STDERR);
#endif
#if defined(REAL_TIME_WINNT)
if(!QueryPerformanceFrequency(&freq))
puts("QueryPerformanceFrequency failed");
#elif defined (REAL_TIME_LINUX)
if(clock_getres(CLOCK_REALTIME, &freq))
puts("clock_getres failed");
#elif defined (REAL_TIME_OSX)
// If this is the first time we've run, get the timebase.
// We can use denom == 0 to indicate that sTimebaseInfo is
// uninitialised because it makes no sense to have a zero
// denominator is a fraction.
if (freq.denom == 0 ) {
(void) mach_timebase_info(&freq);
}
#endif
// Load Matlab robot objects from Matlab files
if(!(errorn=load_robot(ROBOT_VAR_FILE_NAME,ROBOT_BASE_VAR_NAME,&n_robot_joints,&robot_inv_dyn_object)) && \
!(errorn=load_robot(ROBOT_VAR_FILE_NAME,ROBOT_SIMUL_VAR_NAME,NULL,&robot_dir_dyn_object)))
{
if(!(errorn=allocate_integration_buffers(&num_integration_buffers,n_robot_joints))) // Initialize the buffers for numerical integration using the initial desired robot's state
{
// Initialize variable log
if(!(errorn=create_log(&var_log, MAX_TRAJ_EXECUTIONS, TRAJECTORY_TIME)))
{
// Initialize EDLUT and load neural network files
neural_sim=create_neural_simulation(NET_FILE, INPUT_WEIGHT_FILE, INPUT_ACTIVITY_FILE, OUTPUT_WEIGHT_FILE, OUTPUT_ACTIVITY_FILE, WEIGHT_SAVE_PERIOD, NUMBER_OF_OPENMP_QUEUES, NUMBER_OF_OPENMP_THREADS);
if(neural_sim)
{
total_output_spks=0L;
puts("Simulating...");
sim_elapsed_time=0.0;
errorn=0;
// _CrtMemCheckpoint(&state0);
bool real_time_neural_simulation=false;
if(REAL_TIME_NEURAL_SIM==1){
#ifdef _OPENMP
omp_set_nested(true);
real_time_neural_simulation=true;
cout<<"\nFixed REAL TIME SIMULATION option\n"<<endl;
init_real_time_restriction(neural_sim, SIM_SLOT_LENGTH, MAX_SIMULATION_DELAY, FIRST_REAL_TIME_RESTRICTION, SECOND_REAL_TIME_RESTRICTION, THIRD_REAL_TIME_RESTRICTION);
#else
cout<<"\nREAL TIME SIMULATION option is not available due to the openMP support is disabled\n"<<endl;
#endif
}
#pragma omp parallel if(real_time_neural_simulation) num_threads(2)
{
if(omp_get_thread_num()==1){
start_real_time_restriction(neural_sim);
}else{
#pragma omp parallel if(NumberOfOpenMPThreads>1) default(shared) private( n_traj_exec, sim_time, cur_traject_time)
{
//Select the correspondent device.
if(NumberOfGPUs>0){
HANDLE_ERROR(cudaSetDevice(GPUsIndex[openMP_index % NumberOfGPUs]));
}
if(omp_get_thread_num()>0){
for(n_traj_exec=0;n_traj_exec<MAX_TRAJ_EXECUTIONS && !errorn;n_traj_exec++){
cur_traject_time=0.0;
do
{
// control loop iteration starts
sim_time=(double)n_traj_exec*TRAJECTORY_TIME + cur_traject_time; // Calculate absolute simulation time
errorn=run_neural_simulation_slot(neural_sim, sim_time+SIM_SLOT_LENGTH); // Simulation the neural network during a time slot
cur_traject_time+=SIM_SLOT_LENGTH;
}
while(cur_traject_time<TRAJECTORY_TIME-(SIM_SLOT_LENGTH/2.0) && !errorn); // we add -(SIM_SLOT_LENGTH/2.0) because of floating-point-type codification problems
}
}else{
for(n_traj_exec=0;n_traj_exec<MAX_TRAJ_EXECUTIONS && !errorn;n_traj_exec++){
calculate_input_trajectory(robot_state_vars, TRAJ_POS_AMP, 0.0); // Initialize simulated robot's actual state from the desired state (input trajectory) (position, velocity and acceleration)
initialize_integration_buffers(robot_state_vars,&num_integration_buffers,n_robot_joints); // For the robot's direct
init_delay(&cerebellar_learning_delay, ERROR_DELAY_TIME);
//////////////////////////////REVISAR ESTE IF///////////////////////////////////////
if(INPUT_ACTIVITY_FILE==0){
reset_neural_simulation(neural_sim); // after each trajectory execution the network simulation state must be reset (pending activity events are discarded)
////////////////////////////////////////////////////////
double time=((double)n_traj_exec)*TRAJECTORY_TIME;
calculate_input_activity_for_one_trajectory(neural_sim, time);
////////////////////////////////////////////////////////////
}
cur_traject_time=0.0;
if(REAL_TIME_NEURAL_SIM==1){
reset_real_time_restriction(neural_sim);
}
do
{
int n_joint;
if(REAL_TIME_NEURAL_SIM==1){
next_step_real_time_restriction(neural_sim);
}
#if defined(REAL_TIME_WINNT)
QueryPerformanceCounter(&startt);
#elif defined(REAL_TIME_LINUX)
clock_gettime(CLOCK_REALTIME, &startt);
#elif defined(REAL_TIME_OSX)
startt = mach_absolute_time();
#endif
// control loop iteration starts
calculate_input_trajectory(input_traject_vars, TRAJ_POS_AMP, cur_traject_time); // Calculate desired input trajectory
sim_time=(double)n_traj_exec*TRAJECTORY_TIME + cur_traject_time; // Calculate absolute simulation time
compute_robot_inv_dynamics(robot_inv_dyn_object,input_traject_vars,ROBOT_EXTERNAL_FORCE,ROBOT_GRAVITY,robot_inv_dyn_torque); // Calculate crude inverse dynamics of the base robot. They constitude the base robot's input torque
for(n_joint=0;n_joint<NUM_JOINTS;n_joint++) // Calculate total torque from forward controller (cerebellum) torque plus base controller torque
total_torque[n_joint]=robot_inv_dyn_torque[n_joint]+(cerebellar_output_vars[n_joint*2]-cerebellar_output_vars[n_joint*2+1]);
compute_robot_dir_dynamics(robot_dir_dyn_object,robot_state_vars,total_torque,robot_state_vars,&num_integration_buffers,ROBOT_EXTERNAL_FORCE,ROBOT_GRAVITY,SIM_SLOT_LENGTH); // Simulate the robot (direct dynamics).
calculate_error_signals(input_traject_vars, robot_state_vars, robot_error_vars); // Calculated robot's performed error
calculate_learning_signals(robot_error_vars, cerebellar_output_vars, cerebellar_learning_vars); // Calculate learning signal from the calculated error
delayed_cerebellar_learning_vars=delay_line(&cerebellar_learning_delay,cerebellar_learning_vars);
generate_learning_activity(neural_sim, sim_time,delayed_cerebellar_learning_vars);
errorn=run_neural_simulation_slot(neural_sim, sim_time+SIM_SLOT_LENGTH); // Simulation the neural network during a time slot
total_output_spks+=(long)compute_output_activity(neural_sim, cerebellar_output_vars); // Translates cerebellum output activity into analog output variables (corrective torques)
// control loop iteration ends
#if defined(REAL_TIME_WINNT)
QueryPerformanceCounter(&endt); // measures time
slot_elapsed_time=(endt.QuadPart-startt.QuadPart)/(float)freq.QuadPart; // to be logged
#elif defined(REAL_TIME_LINUX)
clock_gettime(CLOCK_REALTIME, &endt);
// Calculate time it took
slot_elapsed_time = (endt.tv_sec-startt.tv_sec ) + (endt.tv_nsec-startt.tv_nsec )/float(1e9);
#elif defined(REAL_TIME_OSX)
// Stop the clock.
endt = mach_absolute_time();
// Calculate the duration.
elapsed = endt - startt;
slot_elapsed_time = 1e-9 * elapsed * freq.numer / freq.denom;
#endif
sim_elapsed_time+=slot_elapsed_time;
log_vars(&var_log, sim_time, input_traject_vars, robot_state_vars, robot_inv_dyn_torque, cerebellar_output_vars, cerebellar_learning_vars, robot_error_vars, slot_elapsed_time,get_neural_simulation_spike_counter_for_each_slot_time()); // Store vars into RAM
cur_traject_time+=SIM_SLOT_LENGTH;
}
while(cur_traject_time<TRAJECTORY_TIME-(SIM_SLOT_LENGTH/2.0) && !errorn); // we add -(SIM_SLOT_LENGTH/2.0) because of floating-point-type codification problems
}
}
if(real_time_neural_simulation){
stop_real_time_restriction(neural_sim);
}
}
}
}
// reset_neural_simulation(neural_sim);
// _CrtMemDumpAllObjectsSince(&state0);
if(errorn)
printf("Error %i performing neural network simulation\n",errorn);
printf("Total neural-network output spikes: %li\n",total_output_spks);
printf("Total number of neural updates: %Ld\n",get_neural_simulation_event_counter(neural_sim));
printf("Mean number of neural-network spikes in heap: %f\n",get_accumulated_heap_occupancy_counter(neural_sim)/(double)get_neural_simulation_event_counter(neural_sim));
for(int i=0; i<Simul.GetNumberOfQueues(); i++){
cout << "Thread "<<i<<"--> Number of updates: " << neural_sim.GetSimulationUpdates(i) << endl; /*asdfgf*/
cout << "Thread "<<i<<"--> Number of InternalSpike: " << neural_sim.GetTotalSpikeCounter(i) << endl; /*asdfgf*/
cout << "Thread "<<i<<"--> Number of Propagated Spikes and Events: " << neural_sim.GetTotalPropagateCounter(i)<<", "<< neural_sim.GetTotalPropagateEventCounter(i)<< endl; /*asdfgf*/
cout << "Thread "<<i<<"--> Mean number of spikes in heap: " << neural_sim.GetHeapAcumSize(i)/(float)neural_sim.GetSimulationUpdates(i) << endl; /*asdfgf*/
}
cout << "Total InternalSpike: " << neural_sim.GetTotalSpikeCounter()<<endl;
cout << "Total Propagated Spikes and Events: " << neural_sim.GetTotalPropagateCounter()<<", "<< Simul.GetTotalPropagateEventCounter()<<endl;
#if defined(REAL_TIME_WINNT)
printf("Total elapsed time: %fs (time resolution: %fus)\n",sim_elapsed_time,1.0e6/freq.QuadPart);
#elif defined(REAL_TIME_LINUX)
printf("Total elapsed time: %fs (time resolution: %fus)\n",sim_elapsed_time,freq.tv_sec*1.0e6+freq.tv_nsec/float(1e3));
#elif defined(REAL_TIME_OSX)
printf("Total elapsed time: %fs (time resolution: %fus)\n",sim_elapsed_time,1e-3*freq.numer/freq.denom);
#endif
save_neural_weights(neural_sim);
finish_neural_simulation(neural_sim);
}
else
{
errorn=10000;
printf("Error initializing neural network simulation\n");
}
puts("Saving log file");
errorn=save_and_finish_log(&var_log, LOG_FILE); // Store logged vars in disk
if(errorn)
printf("Error %i while saving log file\n",errorn);
}
else
{
errorn*=1000;
printf("Error allocating memory for the log of the simulation variables\n");
}
free_robot(robot_inv_dyn_object);
free_robot(robot_dir_dyn_object);
}
else
{
errorn*=10;
printf("Error loading the robot object from file: %s\n",ROBOT_VAR_FILE_NAME);
}
if(!errorn)
puts("OK");
else
printf("Error: %i\n",errorn);
#if defined(_DEBUG) && (defined(_WIN32) || defined(_WIN64))
_CrtDumpMemoryLeaks();
#endif
}else{
printf("Error loading robot: %i\n",errorn);
}
return(errorn);
}