diff --git a/ati_netbox2_lc.lc b/ati_netbox2_lc.lc new file mode 100644 index 0000000000000000000000000000000000000000..2a4226b0269bd8a7ae5a8f126bc19e47504e2402 --- /dev/null +++ b/ati_netbox2_lc.lc @@ -0,0 +1,4 @@ +sample struct { + long info[3]; + float forcesTorques[6]; +} ati2rob_2; diff --git a/ati_netbox_lc.lc b/ati_netbox_lc.lc new file mode 100644 index 0000000000000000000000000000000000000000..6211ade0b9f3b2d7e694731b2c98ca8fc938d6ad --- /dev/null +++ b/ati_netbox_lc.lc @@ -0,0 +1,4 @@ +sample struct { + long info[3]; + float forcesTorques[6]; +} ati2rob; diff --git a/coupling.lc b/coupling.lc new file mode 100644 index 0000000000000000000000000000000000000000..a15f23644c1f1e52655e3317567b146d19913571 --- /dev/null +++ b/coupling.lc @@ -0,0 +1,8 @@ +//robot to cpp +sample float yaFromRobot[28]; + + +//cpp to robot +sample float yarddToRobot[14]; +sample float optsToRobot[100]; + diff --git a/cpp/Makefile b/cpp/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..1ef37769997a6b6b5f016bf88f91c2db1e08d63f --- /dev/null +++ b/cpp/Makefile @@ -0,0 +1,36 @@ +CC=g++ +CFLAGS=-I. -Wall -I/opt/robot/labcomm -I/opt/robot/orca \ +-fpermissive -lpthread -std=c++11 -I/home/martinka/miscellaneous/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -llapack -lblas +LFLAGS=-lrt +LIBS=-lm -llapack -lblas +VPATH=/opt/robot/orca /opt/robot/labcomm +OBJSTATIC = orca_client.o orca_messages.o labcomm.o coupling.o traj2dmp.o \ +dmp.o armmoving.o state2yardd.o record_ya.o twod_controller.o write_norms_to_file.o + +all: main +%: %.cc + $(CC) -o $@ $(CFLAGS) $^ $(LIBS) + + +%.o: %.cc $(DEPS) + $(CC) $(CFLAGS) -c -o $@ $< + +%.h %.c: %.lc + labcomm -C $< + + +main: $(OBJSTATIC) main.cc + $(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) main.cc -o main + +sim: $(OBJSTATIC) sim.cc + $(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) sim.cc -o sim + +static: $(OBJSTATIC) + ar rcs libcmt.a $^ + +clean: + rm -f main + rm -f *.o \ + rm -f libcmt.a + rm -f libcmt.so + diff --git a/cpp/armmoving.cc b/cpp/armmoving.cc new file mode 100644 index 0000000000000000000000000000000000000000..44f26f7506ca016a2ead2b48ec28fcd7bf7b74a4 --- /dev/null +++ b/cpp/armmoving.cc @@ -0,0 +1,35 @@ +#include "armmoving.h" +#include <iostream> +#include <armadillo> + +using namespace arma; + + + +bool armMoving(const mat& jointPos) { + static bool ismoving = false; + const double th = 0.002; + + + static int counter = 0; + static mat jointPosOld = zeros<mat>(1,dof); + + if (counter == 0) { // first run + jointPosOld = jointPos; + } + + + if (counter == 50) { + counter = 1; + if ( all(vectorise(sum(abs(jointPosOld - jointPos),1) > th))) { + ismoving = true; + } else { + ismoving = false; + } + jointPosOld = jointPos; + } + + + ++counter; + return ismoving; +} diff --git a/cpp/armmoving.h b/cpp/armmoving.h new file mode 100644 index 0000000000000000000000000000000000000000..9721884b9473f670c55a72cc6971460f848cb97a --- /dev/null +++ b/cpp/armmoving.h @@ -0,0 +1,10 @@ +#include <armadillo> +#include "dmp.h" +using namespace arma; +#ifndef ARMMOVING_H +#define ARMMOVING_H + +bool armMoving(const mat& jointPos); + + +#endif diff --git a/cpp/coupling.c b/cpp/coupling.c new file mode 100644 index 0000000000000000000000000000000000000000..ff9997fb423ef1736dc2859d93a1061febc85381 --- /dev/null +++ b/cpp/coupling.c @@ -0,0 +1,265 @@ +#include "labcomm.h" +#include "labcomm_private.h" +#include "coupling.h" + +static unsigned char signature_bytes_yaFromRobot[] = { +// array [28] +0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 28, + 0, 0, 0, 37, +// } +}; +labcomm_signature_t labcomm_signature_coupling_yaFromRobot = { + LABCOMM_SAMPLE, "yaFromRobot", + (int (*)(void *))labcomm_sizeof_coupling_yaFromRobot, + sizeof(signature_bytes_yaFromRobot), + signature_bytes_yaFromRobot + }; +static unsigned char signature_bytes_yarddToRobot[] = { +// array [14] +0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 14, + 0, 0, 0, 37, +// } +}; +labcomm_signature_t labcomm_signature_coupling_yarddToRobot = { + LABCOMM_SAMPLE, "yarddToRobot", + (int (*)(void *))labcomm_sizeof_coupling_yarddToRobot, + sizeof(signature_bytes_yarddToRobot), + signature_bytes_yarddToRobot + }; +static unsigned char signature_bytes_optsToRobot[] = { +// array [100] +0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 100, + 0, 0, 0, 37, +// } +}; +labcomm_signature_t labcomm_signature_coupling_optsToRobot = { + LABCOMM_SAMPLE, "optsToRobot", + (int (*)(void *))labcomm_sizeof_coupling_optsToRobot, + sizeof(signature_bytes_optsToRobot), + signature_bytes_optsToRobot + }; +static void decode_yaFromRobot( + labcomm_decoder_t *d, + void (*handle)( + coupling_yaFromRobot *v, + void *context + ), + void *context +) +{ + coupling_yaFromRobot v; + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 28 ; i_0_0++) { + v.a[i_0_0] = labcomm_decode_float(d); + } + } + handle(&v, context); +} +void labcomm_decoder_register_coupling_yaFromRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_yaFromRobot *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_coupling_yaFromRobot, + (labcomm_decoder_typecast_t)decode_yaFromRobot, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_yaFromRobot( + labcomm_encoder_t *e, + coupling_yaFromRobot *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_coupling_yaFromRobot); + { + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 28 ; i_0_0++) { + labcomm_encode_float(e, (*v).a[i_0_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_coupling_yaFromRobot( +labcomm_encoder_t *e, +coupling_yaFromRobot *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_coupling_yaFromRobot, v); +} +void labcomm_encoder_register_coupling_yaFromRobot( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_coupling_yaFromRobot, + (labcomm_encode_typecast_t)encode_yaFromRobot + ); +} +int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v) +{ + return 116; +} +static void decode_yarddToRobot( + labcomm_decoder_t *d, + void (*handle)( + coupling_yarddToRobot *v, + void *context + ), + void *context +) +{ + coupling_yarddToRobot v; + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) { + v.a[i_0_0] = labcomm_decode_float(d); + } + } + handle(&v, context); +} +void labcomm_decoder_register_coupling_yarddToRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_yarddToRobot *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_coupling_yarddToRobot, + (labcomm_decoder_typecast_t)decode_yarddToRobot, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_yarddToRobot( + labcomm_encoder_t *e, + coupling_yarddToRobot *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_coupling_yarddToRobot); + { + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) { + labcomm_encode_float(e, (*v).a[i_0_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_coupling_yarddToRobot( +labcomm_encoder_t *e, +coupling_yarddToRobot *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_coupling_yarddToRobot, v); +} +void labcomm_encoder_register_coupling_yarddToRobot( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_coupling_yarddToRobot, + (labcomm_encode_typecast_t)encode_yarddToRobot + ); +} +int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v) +{ + return 60; +} +static void decode_optsToRobot( + labcomm_decoder_t *d, + void (*handle)( + coupling_optsToRobot *v, + void *context + ), + void *context +) +{ + coupling_optsToRobot v; + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) { + v.a[i_0_0] = labcomm_decode_float(d); + } + } + handle(&v, context); +} +void labcomm_decoder_register_coupling_optsToRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_optsToRobot *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_coupling_optsToRobot, + (labcomm_decoder_typecast_t)decode_optsToRobot, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_optsToRobot( + labcomm_encoder_t *e, + coupling_optsToRobot *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_coupling_optsToRobot); + { + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) { + labcomm_encode_float(e, (*v).a[i_0_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_coupling_optsToRobot( +labcomm_encoder_t *e, +coupling_optsToRobot *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_coupling_optsToRobot, v); +} +void labcomm_encoder_register_coupling_optsToRobot( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_coupling_optsToRobot, + (labcomm_encode_typecast_t)encode_optsToRobot + ); +} +int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v) +{ + return 404; +} diff --git a/cpp/coupling.h b/cpp/coupling.h new file mode 100644 index 0000000000000000000000000000000000000000..e0eeac5e0483832e2c650f20558dd40feff4923b --- /dev/null +++ b/cpp/coupling.h @@ -0,0 +1,80 @@ +/* LabComm declarations: +sample float yaFromRobot[28]; +sample float yarddToRobot[14]; +sample float optsToRobot[100]; +*/ + + +#ifndef __LABCOMM_coupling_H__ +#define __LABCOMM_coupling_H__ + +#include "labcomm.h" + +#ifndef PREDEFINED_coupling_yaFromRobot +typedef struct { + float a[28]; +} coupling_yaFromRobot; +#endif +void labcomm_decoder_register_coupling_yaFromRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_yaFromRobot *v, + void *context + ), + void *context +); +void labcomm_encoder_register_coupling_yaFromRobot( + struct labcomm_encoder *e); +void labcomm_encode_coupling_yaFromRobot( + struct labcomm_encoder *e, + coupling_yaFromRobot *v +); +extern int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v); + +#ifndef PREDEFINED_coupling_yarddToRobot +typedef struct { + float a[14]; +} coupling_yarddToRobot; +#endif +void labcomm_decoder_register_coupling_yarddToRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_yarddToRobot *v, + void *context + ), + void *context +); +void labcomm_encoder_register_coupling_yarddToRobot( + struct labcomm_encoder *e); +void labcomm_encode_coupling_yarddToRobot( + struct labcomm_encoder *e, + coupling_yarddToRobot *v +); +extern int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v); + +#ifndef PREDEFINED_coupling_optsToRobot +typedef struct { + float a[100]; +} coupling_optsToRobot; +#endif +void labcomm_decoder_register_coupling_optsToRobot( + struct labcomm_decoder *d, + void (*handler)( + coupling_optsToRobot *v, + void *context + ), + void *context +); +void labcomm_encoder_register_coupling_optsToRobot( + struct labcomm_encoder *e); +void labcomm_encode_coupling_optsToRobot( + struct labcomm_encoder *e, + coupling_optsToRobot *v +); +extern int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v); + +#define LABCOMM_FORALL_SAMPLES_coupling(func, sep) \ + func(yaFromRobot, coupling_yaFromRobot) sep \ + func(yarddToRobot, coupling_yarddToRobot) sep \ + func(optsToRobot, coupling_optsToRobot) +#endif diff --git a/cpp/dmp.cc b/cpp/dmp.cc new file mode 100644 index 0000000000000000000000000000000000000000..270fc0584c337452d49c43d5e73667cddb486bb6 --- /dev/null +++ b/cpp/dmp.cc @@ -0,0 +1,49 @@ +#include "dmp.h" +#include <iostream> +#include <armadillo> +using namespace arma; +using namespace std; + + + + +Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {} + +mat Dmp::getW() const { + return weights; +} + +mat Dmp::getG() const { + return goal; +} + +double Dmp::getTau() const { + return tau; +} + +void Dmp::setG(mat g) { + goal = g; +} + +void Dmp::setTau(double t) { + tau = t; +} + +Dmp& Dmp::doubleSpeed() { + tau = tau/2; + return *this; +} + +Dmp& Dmp::speedupTimes(const double& x) { + tau = tau/x; + return *this; +} + + +ostream& operator<<(ostream& os, const Dmp& dmp) { + os << "Weights = " << dmp.weights << endl; + os << "Goal = " << dmp.goal << endl; + os << "Tau = " << dmp.tau << endl; + + return os; +} diff --git a/cpp/dmp.h b/cpp/dmp.h new file mode 100644 index 0000000000000000000000000000000000000000..e9f3d5ef8cd56046c8bf98cfbc3e776613cbabbe --- /dev/null +++ b/cpp/dmp.h @@ -0,0 +1,63 @@ +#include <armadillo> +using namespace arma; +using namespace std; + +#ifndef DMP_H +#define DMP_H + +const int dof = 14; +const double offs = pow(10,-10); + +const double alpha_z = 10; // Velocity gain +const double beta_z = alpha_z / 4; +const int n_kernel = 50; +const mat ct = linspace<mat>(0, 1, n_kernel); +const mat cx = exp(-3*ct); +const mat c = cx; + +class Dmp { + friend ostream& operator<<(ostream&, const Dmp&); +public: + Dmp(mat w, mat g, double t); + mat getW() const; + mat getG() const; + double getTau() const; + + void setG(mat g); + void setTau(double t); + Dmp& doubleSpeed(); + Dmp& speedupTimes(const double& x); + + + + + +private: + mat weights; + mat goal; + double tau; + + //new, to be integrated: + string ID; + string GroupID; + string log_url; + string name; + string comment; + string purpose; + string tool; + string robot; + string timestamp; + string controllertype; //e.g. extctrl + string robotcell; + string movementType; + string startTolerance; + string endTolerance; + string goalObject; +}; + +ostream& operator<<(ostream& os, const Dmp& dmp); + + + + +#endif diff --git a/cpp/k600_driver_3leds_comet.c b/cpp/k600_driver_3leds_comet.c new file mode 100644 index 0000000000000000000000000000000000000000..6917cb6dc56d6f0012ce0039559130cf390c99dc --- /dev/null +++ b/cpp/k600_driver_3leds_comet.c @@ -0,0 +1,390 @@ +// Example: +// ./k600_driver_frames ip_or_name_of_your_extctrl_computer 2000 nbrfr +// +// Start measurement at K600 +// telnet fair-02 50505 + + +#define _POSIX_C_SOURCE 199309 +#include <time.h> +#include <stdlib.h> +#include <stdio.h> +#include <orca_client.h> +#include <pthread.h> +#include "k600data.h" + +#include <sys/select.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <arpa/inet.h> +#include <netinet/in.h> +#include <unistd.h> + +#include <sys/time.h> + + +#include <netdb.h> +#include <string.h> +#include <sys/times.h> + +#define K600SERVER "fair-02" +#define TCPPORT 50506 /* shared client/server tcp port number */ +#define N_LEDS 3 +#define N_FRAMES 0 +#define BUFSIZE 4+8*7*N_FRAMES+8*4*N_LEDS /*size of buffer sent */ +/* See Nikon_doc/JS014_K-Link_TCP_Interface.doc */ + +/* This function which continously receives data from irc5 + * must be started from a thread because the function will + * not end until the connection to irc5 is shut down. */ + +void* handler_thread(void *arg) { + orca_client_channel_t* signalsFrRob = (orca_client_channel_t*) arg; + labcomm_decoder_run(signalsFrRob->decoder); + return(0); +} +//------------------------------------------------------------- + + + /* + * initSocket + * create a tcp socket + * fill in the server structure with + * protocol family, remote address, tcp port + * CALLS: + * gethostbyname - map hostname to host addr + */ +// __ // static +int initSocket(server, remote) + struct sockaddr_in *server; /* server structure for connect */ + char *remote; /* remote hostname */ + { + struct hostent *gethostbyname(); + struct hostent *h; + int sock; + + /* create INTERNET,TCP socket + */ + sock = socket(AF_INET, SOCK_STREAM, 0); + + if ( sock < 0 ) { + perror("socket"); + exit(1); + } + + /* fill in the server structure for the connect call + * + * family: INET meaning tcp/ip/ethernet. + * addr: string that represents INET number for remote system. + * port: string that represents tcp server number. + */ + server->sin_family = AF_INET; + h = 0; + + /* read local inet name of REMOTE machine into + * hostent structure. This buffer must be copied into the + * sockaddr_in server structure. + */ + h = gethostbyname(remote); + if ( h == 0 ) { + fprintf(stderr,"gethostbyname: cannot find host %s\n",remote); + exit(2); + } + + /* + bcopy(h->h_addr, &server->sin_addr, h->h_length); + */ + // CHECK THIS!!! memcpy(&server->sin_addr, h->h_addr, h->h_length); + memcpy(&server->sin_addr, h-> h_addr_list[0], h->h_length); + /* convert host short to network byteorder short + */ + server->sin_port = htons(TCPPORT); + return(sock); + } + + + + +//------------------------------------------------------------- + + /* read from socket. TCP read call issued to kernel + * may return with less data than you expected. This routine + * must check that and force a complete read. + */ +int doRead(sock, buf,amountNeeded) + int sock; + char *buf; + int amountNeeded; + { + // register int i; + int rc; + char *bpt; + int count = amountNeeded; + int amtread; + + bpt = buf; + amtread = 0; + + again: + if ( (rc = read(sock,bpt,count)) <= 0 ) { + perror("doRead: reading socket stream"); + exit(1); + } + if (rc !=BUFSIZE) { + printf("=============== rc= %d,\n",rc); + } + + amtread += rc; + + if ( amtread < amountNeeded ) { + count = count - rc; + bpt = bpt + rc; + goto again; + } + return(amountNeeded); + } + +// __ // /* +// __ // * dump a buffer in hex +// __ // */ +// __ // printBuf(buf,size) +// __ // char *buf; +// __ // { +// __ // int i; +// __ // i = 16; +// __ // while(size--) { +// __ // printf("%x", (int)(*buf++&0xff)); +// __ // if ( i == 0 ) { +// __ // i = 16; +// __ // printf("\n"); +// __ // } +// __ // else +// __ // i--; +// __ // } +// __ // } +// __ // + + + +//------------------------------------------------------------------ + +pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +static int sock; +static char buf[BUFSIZE]; + + +/* Is called when robotvec is received from orca/robot. */ +void handle_robotvec(k600data_robotvec* v, void* context) { +static int my_counter = 0; + if(my_counter++ > 10){ + my_counter=0; + //printf("robotvec[0] : %f \n", v->a[0] ); + } +} + +/* Is called when robotvec is received from orca/robot. */ +void *nikon_reader(void *arg) { + + orca_client_channel_t* signalsToRobot = (orca_client_channel_t*) arg; + int ok; + int nbrofdata,j; + k600data_nikon store_k600vec; + + for (j = 0 ; j < 12 ; j++) { + store_k600vec.a[j].x=(double)1.10101; + store_k600vec.a[j].y=(double)1.20101; + store_k600vec.a[j].z=(double)1.30101; + store_k600vec.a[j].roll =(double)1.40101; + store_k600vec.a[j].pitch =(double)1.50101; + store_k600vec.a[j].yaw =(double)1.60101; + store_k600vec.a[j].status=100*j+j; + } + +int n_counter = 0; + + while(1){ + + ok=doRead(sock, buf, BUFSIZE); + if (ok){ + nbrofdata=*((int*)(buf)); + //printf("nbr_of_data %d \n", nbrofdata ); + + //printf("time_stamp %d \n", *((int*)(buf)) ); + //pthread_mutex_lock(&mutex); + + j=0 ; store_k600vec.a[0].x = *((double*)(buf+4+8*(j))); + j=1 ; store_k600vec.a[0].y = *((double*)(buf+4+8*(j))); + j=2 ; store_k600vec.a[0].z = *((double*)(buf+4+8*(j))); + j=3 ; store_k600vec.a[0].status= *((int*)(buf+4+8*(j))); + + j=4 ; store_k600vec.a[1].x = *((double*)(buf+4+8*(j))); + j=5 ; store_k600vec.a[1].y = *((double*)(buf+4+8*(j))); + j=6 ; store_k600vec.a[1].z = *((double*)(buf+4+8*(j))); + j=7 ; store_k600vec.a[1].status= *((int*)(buf+4+8*(j))); + + j=8 ; store_k600vec.a[2].x = *((double*)(buf+4+8*(j))); + j=9 ; store_k600vec.a[2].y = *((double*)(buf+4+8*(j))); + j=10 ; store_k600vec.a[2].z = *((double*)(buf+4+8*(j))); + j=11 ; store_k600vec.a[2].status= *((int*)(buf+4+8*(j))); + + + + + + + if(n_counter++ > 1000){ + n_counter = 0; + printf("nbr_of_data %d \n", nbrofdata ); + printf("store_k600vec.a[0].x %f \n", store_k600vec.a[0].x); + printf("store_k600vec.a[0].y %f \n", store_k600vec.a[0].y); + printf("store_k600vec.a[0].z %f \n", store_k600vec.a[0].z); + printf("store_k600vec.a[0].roll %f \n", store_k600vec.a[0].roll); + printf("store_k600vec.a[0].pitch %f \n", store_k600vec.a[0].pitch); + printf("store_k600vec.a[0].yaw %f \n", store_k600vec.a[0].yaw); + printf("store_k600vec.a[0].status %d \n", store_k600vec.a[0].status); + + printf("store_k600vec.a[5].x %f \n", store_k600vec.a[5].x); + printf("store_k600vec.a[5].y %f \n", store_k600vec.a[5].y); + printf("store_k600vec.a[5].z %f \n", store_k600vec.a[5].z); + printf("store_k600vec.a[5].status %d \n", store_k600vec.a[5].status); + + } + + + labcomm_encode_k600data_nikon(signalsToRobot->encoder, &store_k600vec); + //} + // pthread_mutex_unlock(&mutex); + } + } + return(0); +} + +//------------------------------------------------------------- + +int main(int argc, char *argv[]) +{ + + struct orca_client *orca; + orca_client_channel_t *signals2RobNikon; + orca_client_channel_t *signals2RobMyvec; + orca_client_channel_t *signalsFrRob; + pthread_t thread, thread_nikon; + int iret1,iret2; + + struct sockaddr_in server; + char *remote= K600SERVER; + + struct timespec delay; + delay.tv_sec = 0; + delay.tv_nsec = 4000000; //4 ms + + + if (argc<2){ + printf("Example: \n"); + printf("./k600_driver ip_or_name_of_your_extctrl_computer 2000 \n"); + } + else{ + printf("Start k600_driver\n"); + orca = orca_client_new_tcp(argv[1], atoi(argv[2])); + printf("BB\n"); + if (!orca) { + printf("Connection to %s:%d failed\n", argv[1], atoi(argv[1])); + return 1; + } + + { + sock = initSocket(&server, remote); + /* connect to remote server */ + if (connect(sock, (struct sockaddr*)(&server), sizeof(server)) < 0 ) { + perror("connecting stream socket"); + exit(1); + } + + } + + { + orca_client_selection_t selection; + char *channel[1]; + + channel[0] = "nikon"; // see lc-file + selection.n_0 = 1; + selection.a = channel; + signals2RobNikon = orca_client_select_input_tcp(orca, &selection); + labcomm_encoder_register_k600data_nikon(signals2RobNikon->encoder); + } + { + orca_client_selection_t selection; + char *channel[1]; + + channel[0] = "myvec"; // see lc-file + selection.n_0 = 1; + selection.a = channel; + signals2RobMyvec = orca_client_select_input_tcp(orca, &selection); + labcomm_encoder_register_k600data_myvec(signals2RobMyvec->encoder); + } + + { + orca_client_selection_t selectionFrRob; + char *channelFrRob= "robotvec"; + char **channel_pFrRob = &channelFrRob; + selectionFrRob.n_0 = 1; + selectionFrRob.a = channel_pFrRob; + signalsFrRob = orca_client_select_output_tcp(orca, &selectionFrRob); + labcomm_decoder_register_k600data_robotvec(signalsFrRob->decoder,handle_robotvec,0); + } + //------------------------------------------------------------------------ + + // Create an independent thread that deals with receiving from the robot + iret1 = pthread_create(&thread, NULL, handler_thread, (void*) signalsFrRob); + if (iret1){ + printf("Could not create handler_thread.\n"); + exit(1); + } + iret2 = pthread_create(&thread_nikon, NULL, nikon_reader, (void*) signals2RobNikon); + if (iret2){ + printf("Could not create nikon_thread.\n"); + exit(1); + } + + //------------------------------------------------------------------------ + if (1) { + + int j; + k600data_myvec vec13; + + + printf("CC\n"); + + while (1) { + + // printf("\r\nAngle: "); + // scanf("%f", &angle); + // vec13.a[1] = angle; + + for (j = 0 ; j < 13 ; j++) { + vec13.a[j] = (float)j+1.00012345; + } + +// __// pthread_mutex_lock(&mutex); +// __// pthread_mutex_unlock(&mutex); + + + labcomm_encode_k600data_myvec(signals2RobMyvec->encoder, &vec13); + +#if 0 + { + int my_counter2 = 0; + + if(my_counter2++ > 100){ + my_counter2=0; + printf("qwerty %f [ms]\n", vec13.a[1]); + } + } +#endif + nanosleep(&delay, 0); + } + } + } + return(0); +} + diff --git a/cpp/k600data.lc b/cpp/k600data.lc new file mode 100644 index 0000000000000000000000000000000000000000..20a1811a4025fb291aac9727ed2605213992dc88 --- /dev/null +++ b/cpp/k600data.lc @@ -0,0 +1,13 @@ +sample float myvec[13]; +sample float robotvec[20]; + +typedef struct { +double x; +double y; +double z; +double roll; +double pitch; +double yaw; +int status; +} k600_package; +sample k600_package nikon[12]; diff --git a/cpp/main.cc b/cpp/main.cc new file mode 100644 index 0000000000000000000000000000000000000000..eaef22f4a3efdeb80e4755a5e783e43482d4f4da --- /dev/null +++ b/cpp/main.cc @@ -0,0 +1,294 @@ +#include "dmp.h" +#include <time.h> +#include <stdlib.h> +#include <stdio.h> +#include "orca_client.h" +#include <pthread.h> +#include "coupling.h" + + +#include <sys/select.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <arpa/inet.h> +#include <netinet/in.h> +#include <unistd.h> + +#include <sys/time.h> +#include <vector> + +#include <netdb.h> +#include <string.h> +#include <sys/times.h> +#include <iostream> + + +#include <unistd.h> +#include <mutex> +#include <thread> +#include <time.h> +#include <armadillo> +#include "armmoving.h" +#include "record_ya.h" +#include "state2yardd.h" +#include "write_norms_to_file.h" + + + + +using namespace std; + +mutex mtx; +// Some global variables. Sry. To be changed. But perhaps the dmpVector should be kept global. + +vector<Dmp> dmpVec; +mat yardd(1,dof); +mat yclog(1,dof); +mat ycddlog(1,dof); + +// Log convergence states +mat ya_minus_yc(1,dof); +mat yad_minus_ycd(1,dof); +double e_norm_log; +double xlog; +mat yc_minus_g(1,dof); +mat zlog(1,dof); + + + + + +//Global constants: +const double dt = 0.004; +const int takeEvery = 50; +const double sparse_dt = takeEvery*dt; + + + +// Some global variable settings. Sorry about that. Will be changed. +int DMP_NBR = 0; +int RECORD_DMP = 0; +int PLAY_DMP = 0; + + + +/* This function which continously receives data from irc5 + * must be started from a thread because the function will + * not end until the connection to irc5 is shut down. */ + +void* handler_thread(void *arg) { + orca_client_channel_t* signalsFromRobot = (orca_client_channel_t*) arg; + labcomm_decoder_run(signalsFromRobot->decoder); + return(0); +} + + + +//------------------------------------------------------------------ +/* Is called when trajectory is received from orca/robot. */ +void handle_trajectory(coupling_yaFromRobot* v, void* context) { + + + mat ya(1,dof); + for (int i = 0; i < dof; ++i) { + ya(i) = v->a[i]; + } + mat yad(1,dof); + for (int i = dof; i < 2*dof; ++i) { + yad(i-dof) = v->a[i]; + } + + + + + mtx.lock(); + record_ya(ya, dmpVec, dt, sparse_dt, takeEvery, RECORD_DMP, PLAY_DMP); + mtx.unlock(); + + mtx.lock(); + state2yardd(ya,yad,yardd, PLAY_DMP, DMP_NBR, dmpVec, yclog, ycddlog, ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog); + mtx.unlock(); + + mtx.lock(); + write_norms_to_file(ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog, PLAY_DMP); + mtx.unlock(); + + + +} +//------------------------------------------------------------- + +void runMenu() { + + cout << "OPTIONS: " << endl; + cout << "Record demonstrations on/off - type r" << endl; + cout << "Play next dmp - type p" << endl; + cout << "Stop playing DMP - type s" << endl; + cout << "Choose DMP to play - type a number" << endl; + + cout << "Choose an option: "; + char option; + while (cin >> option) { + if (option == 'p') { //play next dmp + int dmpNbr = DMP_NBR+1; + if (dmpNbr > 0 && dmpNbr < dmpVec.size()+1) { + DMP_NBR = dmpNbr; + PLAY_DMP = 1; + RECORD_DMP = 0; + } else { + cout << "Sorry, no such dmp available" << endl; + } + } else if (option == 'r') { //change value of RECORD_DMP + mtx.lock(); + RECORD_DMP = !RECORD_DMP; + if (RECORD_DMP) { + cout << "Recording" << endl; + } else { + cout << "Not recording" << endl; + } + mtx.unlock(); + } else if (option == 's') { //stop playing dmp + mtx.lock(); + RECORD_DMP = 0; + PLAY_DMP = 0; + mtx.unlock(); + } else if (option -48 >= 0 && option - 48 < 10) { // This is ugly and should be changed. + int dmpNbr = option-48; + mtx.lock(); + if (dmpNbr > 0 && dmpNbr < dmpVec.size()+1) { + DMP_NBR = dmpNbr; + PLAY_DMP = 1; + RECORD_DMP = 0; + } else { + cout << "Sorry, no such dmp available" << endl; + } + mtx.unlock(); + } + + + + cout << endl; + usleep(100000); + mtx.lock(); + if (dmpVec.size() == 0) { + cout << "We have 0 dmp:s" << endl; + } else { + cout << "We have 1..." << dmpVec.size() << " dmp:s" << endl; + } + mtx.unlock(); + } +} + + +//------------------------------------------------------------- + +int main(int argc, char *argv[]) { + struct orca_client *orca; + orca_client_channel_t *signalsFromRobotYa; + orca_client_channel_t *signalsFromRobotYad; + orca_client_channel_t *signals2RobYardd; + orca_client_channel_t *signals2RobOpts; + + pthread_t thread_robot; + int iret1; + + if (argc<2){ + printf("Example: \n"); + printf("./fit_dmp_labcomm_driver ip_or_name_of_your_extctrl_computer 2000 \n"); + } + else{ + printf("Start fit_dmp_labcomm_driver\n"); + orca = orca_client_new_tcp(argv[1], atoi(argv[2])); + printf("BB\n"); + } + if (!orca) { + printf("Connection to %s:%d failed\n", argv[1], atoi(argv[1])); + return 1; + } + + + + // Register to robot: + { + orca_client_selection_t selection; + char *channel[1]; + + channel[0] = "yarddToRobot"; // see lc-file + selection.n_0 = 1; + selection.a = channel; + signals2RobYardd = orca_client_select_input_tcp(orca, &selection); + labcomm_encoder_register_coupling_yarddToRobot(signals2RobYardd->encoder); + } + { + orca_client_selection_t selection; + char *channel[1]; + + channel[0] = "optsToRobot"; // see lc-file + selection.n_0 = 1; + selection.a = channel; + signals2RobOpts = orca_client_select_input_tcp(orca, &selection); + labcomm_encoder_register_coupling_optsToRobot(signals2RobOpts->encoder); + } + + // Register from robot: + { + orca_client_selection_t selectionFrRob; + char *channelFrRob= "yaFromRobot"; + char **channel_pFrRob = &channelFrRob; + selectionFrRob.n_0 = 1; + selectionFrRob.a = channel_pFrRob; + signalsFromRobotYa = orca_client_select_output_tcp(orca, &selectionFrRob); + labcomm_decoder_register_coupling_yaFromRobot(signalsFromRobotYa->decoder,handle_trajectory,0); + } + + // Create an independent thread that deals with receiving from the robot + iret1 = pthread_create(&thread_robot, NULL, handler_thread, (void*) signalsFromRobotYa); + if (iret1){ + printf("Could not create handler_thread.\n"); + exit(1); + } + + coupling_optsToRobot opts_store; + coupling_yarddToRobot yardd_store; + thread menuThread(runMenu); + menuThread.detach(); + + //timeval tv; + + while(1){ + //gettimeofday(&tv, 0); + //int start = tv.tv_usec; + + usleep(4000); + + + + mtx.lock(); + for (int i = 0 ; i < dof ; i++) { + yardd_store.a[i] = yardd(i); + } + for (int i = 0 ; i < 100 ; i++) { + opts_store.a[i] = 0; + } + opts_store.a[0] = PLAY_DMP; + opts_store.a[99] = RECORD_DMP; + + labcomm_encode_coupling_yarddToRobot(signals2RobYardd->encoder, &yardd_store); + labcomm_encode_coupling_optsToRobot(signals2RobOpts->encoder, &opts_store); + mtx.unlock(); + static int count = 1; + if (PLAY_DMP && count == 250) { + cout << yardd << endl; + count = 1; + } + if (PLAY_DMP) { + ++count; + } + //gettimeofday(&tv, 0); + //cout << tv.tv_usec - start << endl; + } + + + printf("Shutting down\n"); + return(0); +} diff --git a/cpp/orca_messages.c b/cpp/orca_messages.c new file mode 100644 index 0000000000000000000000000000000000000000..cdcc077ccbec4e3b343a5113fd43f27f6fb01c95 --- /dev/null +++ b/cpp/orca_messages.c @@ -0,0 +1,880 @@ +#include "labcomm.h" +#include "labcomm_private.h" +#include "orca_messages.h" + +static unsigned char signature_bytes_directory[] = { +// struct { 4 fields +0, 0, 0, 17, + 0, 0, 0, 4, + // connection_list_t 'input' + 0, 0, 0, 5, + 105, 110, 112, 117, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + // struct { 3 fields + 0, 0, 0, 17, + 0, 0, 0, 3, + // int 'index' + 0, 0, 0, 5, + 105, 110, 100, 101, 120, + 0, 0, 0, 35, + // string 'name' + 0, 0, 0, 4, + 110, 97, 109, 101, + 0, 0, 0, 39, + // array [_] 'signature' + 0, 0, 0, 9, + 115, 105, 103, 110, 97, 116, 117, 114, 101, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 33, + // } + // } + // } + // connection_list_t 'output' + 0, 0, 0, 6, + 111, 117, 116, 112, 117, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + // struct { 3 fields + 0, 0, 0, 17, + 0, 0, 0, 3, + // int 'index' + 0, 0, 0, 5, + 105, 110, 100, 101, 120, + 0, 0, 0, 35, + // string 'name' + 0, 0, 0, 4, + 110, 97, 109, 101, + 0, 0, 0, 39, + // array [_] 'signature' + 0, 0, 0, 9, + 115, 105, 103, 110, 97, 116, 117, 114, 101, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 33, + // } + // } + // } + // connection_list_t 'parameter' + 0, 0, 0, 9, + 112, 97, 114, 97, 109, 101, 116, 101, 114, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + // struct { 3 fields + 0, 0, 0, 17, + 0, 0, 0, 3, + // int 'index' + 0, 0, 0, 5, + 105, 110, 100, 101, 120, + 0, 0, 0, 35, + // string 'name' + 0, 0, 0, 4, + 110, 97, 109, 101, + 0, 0, 0, 39, + // array [_] 'signature' + 0, 0, 0, 9, + 115, 105, 103, 110, 97, 116, 117, 114, 101, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 33, + // } + // } + // } + // connection_list_t 'log' + 0, 0, 0, 3, + 108, 111, 103, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + // struct { 3 fields + 0, 0, 0, 17, + 0, 0, 0, 3, + // int 'index' + 0, 0, 0, 5, + 105, 110, 100, 101, 120, + 0, 0, 0, 35, + // string 'name' + 0, 0, 0, 4, + 110, 97, 109, 101, + 0, 0, 0, 39, + // array [_] 'signature' + 0, 0, 0, 9, + 115, 105, 103, 110, 97, 116, 117, 114, 101, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 33, + // } + // } + // } +// } +}; +labcomm_signature_t labcomm_signature_orca_messages_directory = { + LABCOMM_SAMPLE, "directory", + (int (*)(void *))labcomm_sizeof_orca_messages_directory, + sizeof(signature_bytes_directory), + signature_bytes_directory + }; +static unsigned char signature_bytes_select_input[] = { +// struct { 3 fields +0, 0, 0, 17, + 0, 0, 0, 3, + // int 'port' + 0, 0, 0, 4, + 112, 111, 114, 116, + 0, 0, 0, 35, + // int 'decimation' + 0, 0, 0, 10, + 100, 101, 99, 105, 109, 97, 116, 105, 111, 110, + 0, 0, 0, 35, + // array [_] 'list' + 0, 0, 0, 4, + 108, 105, 115, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 35, + // } +// } +}; +labcomm_signature_t labcomm_signature_orca_messages_select_input = { + LABCOMM_SAMPLE, "select_input", + (int (*)(void *))labcomm_sizeof_orca_messages_select_input, + sizeof(signature_bytes_select_input), + signature_bytes_select_input + }; +static unsigned char signature_bytes_select_output[] = { +// struct { 3 fields +0, 0, 0, 17, + 0, 0, 0, 3, + // int 'port' + 0, 0, 0, 4, + 112, 111, 114, 116, + 0, 0, 0, 35, + // int 'decimation' + 0, 0, 0, 10, + 100, 101, 99, 105, 109, 97, 116, 105, 111, 110, + 0, 0, 0, 35, + // array [_] 'list' + 0, 0, 0, 4, + 108, 105, 115, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 35, + // } +// } +}; +labcomm_signature_t labcomm_signature_orca_messages_select_output = { + LABCOMM_SAMPLE, "select_output", + (int (*)(void *))labcomm_sizeof_orca_messages_select_output, + sizeof(signature_bytes_select_output), + signature_bytes_select_output + }; +static unsigned char signature_bytes_select_parameter[] = { +// struct { 3 fields +0, 0, 0, 17, + 0, 0, 0, 3, + // int 'port' + 0, 0, 0, 4, + 112, 111, 114, 116, + 0, 0, 0, 35, + // int 'decimation' + 0, 0, 0, 10, + 100, 101, 99, 105, 109, 97, 116, 105, 111, 110, + 0, 0, 0, 35, + // array [_] 'list' + 0, 0, 0, 4, + 108, 105, 115, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 35, + // } +// } +}; +labcomm_signature_t labcomm_signature_orca_messages_select_parameter = { + LABCOMM_SAMPLE, "select_parameter", + (int (*)(void *))labcomm_sizeof_orca_messages_select_parameter, + sizeof(signature_bytes_select_parameter), + signature_bytes_select_parameter + }; +static unsigned char signature_bytes_select_log[] = { +// struct { 3 fields +0, 0, 0, 17, + 0, 0, 0, 3, + // int 'port' + 0, 0, 0, 4, + 112, 111, 114, 116, + 0, 0, 0, 35, + // int 'decimation' + 0, 0, 0, 10, + 100, 101, 99, 105, 109, 97, 116, 105, 111, 110, + 0, 0, 0, 35, + // array [_] 'list' + 0, 0, 0, 4, + 108, 105, 115, 116, + // array [_] + 0, 0, 0, 16, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 35, + // } +// } +}; +labcomm_signature_t labcomm_signature_orca_messages_select_log = { + LABCOMM_SAMPLE, "select_log", + (int (*)(void *))labcomm_sizeof_orca_messages_select_log, + sizeof(signature_bytes_select_log), + signature_bytes_select_log + }; +static void decode_directory( + labcomm_decoder_t *d, + void (*handle)( + orca_messages_directory *v, + void *context + ), + void *context +) +{ + orca_messages_directory v; + v.input.n_0 = labcomm_decode_int(d); + v.input.a = malloc(sizeof(v.input.a[0]) * v.input.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.input.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.input.a[i_0].index = labcomm_decode_int(d); + v.input.a[i_0].name = labcomm_decode_string(d); + v.input.a[i_0].signature.n_0 = labcomm_decode_int(d); + v.input.a[i_0].signature.a = malloc(sizeof(v.input.a[i_0].signature.a[0]) * v.input.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < v.input.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + v.input.a[i_0].signature.a[i_1] = labcomm_decode_byte(d); + } + } + } + } + v.output.n_0 = labcomm_decode_int(d); + v.output.a = malloc(sizeof(v.output.a[0]) * v.output.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.output.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.output.a[i_0].index = labcomm_decode_int(d); + v.output.a[i_0].name = labcomm_decode_string(d); + v.output.a[i_0].signature.n_0 = labcomm_decode_int(d); + v.output.a[i_0].signature.a = malloc(sizeof(v.output.a[i_0].signature.a[0]) * v.output.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < v.output.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + v.output.a[i_0].signature.a[i_1] = labcomm_decode_byte(d); + } + } + } + } + v.parameter.n_0 = labcomm_decode_int(d); + v.parameter.a = malloc(sizeof(v.parameter.a[0]) * v.parameter.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.parameter.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.parameter.a[i_0].index = labcomm_decode_int(d); + v.parameter.a[i_0].name = labcomm_decode_string(d); + v.parameter.a[i_0].signature.n_0 = labcomm_decode_int(d); + v.parameter.a[i_0].signature.a = malloc(sizeof(v.parameter.a[i_0].signature.a[0]) * v.parameter.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < v.parameter.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + v.parameter.a[i_0].signature.a[i_1] = labcomm_decode_byte(d); + } + } + } + } + v.log.n_0 = labcomm_decode_int(d); + v.log.a = malloc(sizeof(v.log.a[0]) * v.log.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.log.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.log.a[i_0].index = labcomm_decode_int(d); + v.log.a[i_0].name = labcomm_decode_string(d); + v.log.a[i_0].signature.n_0 = labcomm_decode_int(d); + v.log.a[i_0].signature.a = malloc(sizeof(v.log.a[i_0].signature.a[0]) * v.log.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < v.log.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + v.log.a[i_0].signature.a[i_1] = labcomm_decode_byte(d); + } + } + } + } + handle(&v, context); + { + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.input.n_0 ; i_0_0++) { + int i_0 = i_0_0; + free(v.input.a[i_0].name); + free(v.input.a[i_0].signature.a); + } + } + free(v.input.a); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.output.n_0 ; i_0_0++) { + int i_0 = i_0_0; + free(v.output.a[i_0].name); + free(v.output.a[i_0].signature.a); + } + } + free(v.output.a); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.parameter.n_0 ; i_0_0++) { + int i_0 = i_0_0; + free(v.parameter.a[i_0].name); + free(v.parameter.a[i_0].signature.a); + } + } + free(v.parameter.a); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.log.n_0 ; i_0_0++) { + int i_0 = i_0_0; + free(v.log.a[i_0].name); + free(v.log.a[i_0].signature.a); + } + } + free(v.log.a); + } +} +void labcomm_decoder_register_orca_messages_directory( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_directory *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_orca_messages_directory, + (labcomm_decoder_typecast_t)decode_directory, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_directory( + labcomm_encoder_t *e, + orca_messages_directory *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_orca_messages_directory); + { + labcomm_encode_int(e, (*v).input.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).input.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).input.a[i_0].index); + labcomm_encode_string(e, (*v).input.a[i_0].name); + labcomm_encode_int(e, (*v).input.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < (*v).input.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + labcomm_encode_byte(e, (*v).input.a[i_0].signature.a[i_1]); + } + } + } + } + labcomm_encode_int(e, (*v).output.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).output.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).output.a[i_0].index); + labcomm_encode_string(e, (*v).output.a[i_0].name); + labcomm_encode_int(e, (*v).output.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < (*v).output.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + labcomm_encode_byte(e, (*v).output.a[i_0].signature.a[i_1]); + } + } + } + } + labcomm_encode_int(e, (*v).parameter.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).parameter.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).parameter.a[i_0].index); + labcomm_encode_string(e, (*v).parameter.a[i_0].name); + labcomm_encode_int(e, (*v).parameter.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < (*v).parameter.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + labcomm_encode_byte(e, (*v).parameter.a[i_0].signature.a[i_1]); + } + } + } + } + labcomm_encode_int(e, (*v).log.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).log.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).log.a[i_0].index); + labcomm_encode_string(e, (*v).log.a[i_0].name); + labcomm_encode_int(e, (*v).log.a[i_0].signature.n_0); + { + int i_1_0; + for (i_1_0 = 0 ; i_1_0 < (*v).log.a[i_0].signature.n_0 ; i_1_0++) { + int i_1 = i_1_0; + labcomm_encode_byte(e, (*v).log.a[i_0].signature.a[i_1]); + } + } + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_orca_messages_directory( +labcomm_encoder_t *e, +orca_messages_directory *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_orca_messages_directory, v); +} +void labcomm_encoder_register_orca_messages_directory( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_orca_messages_directory, + (labcomm_encode_typecast_t)encode_directory + ); +} +int labcomm_sizeof_orca_messages_directory(orca_messages_directory *v) +{ + int result = 4; + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).input.n_0 ; i_0_0++) { + int i_0 = i_0_0; + result += 4 + strlen((*v).input.a[i_0].name); + result += 1 * (*v).input.a[i_0].signature.n_0; + result += 4; + } + } + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).output.n_0 ; i_0_0++) { + int i_0 = i_0_0; + result += 4 + strlen((*v).output.a[i_0].name); + result += 1 * (*v).output.a[i_0].signature.n_0; + result += 4; + } + } + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).parameter.n_0 ; i_0_0++) { + int i_0 = i_0_0; + result += 4 + strlen((*v).parameter.a[i_0].name); + result += 1 * (*v).parameter.a[i_0].signature.n_0; + result += 4; + } + } + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).log.n_0 ; i_0_0++) { + int i_0 = i_0_0; + result += 4 + strlen((*v).log.a[i_0].name); + result += 1 * (*v).log.a[i_0].signature.n_0; + result += 4; + } + } + return result; +} +static void decode_select_input( + labcomm_decoder_t *d, + void (*handle)( + orca_messages_select_input *v, + void *context + ), + void *context +) +{ + orca_messages_select_input v; + v.port = labcomm_decode_int(d); + v.decimation = labcomm_decode_int(d); + v.list.n_0 = labcomm_decode_int(d); + v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.list.a[i_0] = labcomm_decode_int(d); + } + } + handle(&v, context); + { + free(v.list.a); + } +} +void labcomm_decoder_register_orca_messages_select_input( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_input *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_orca_messages_select_input, + (labcomm_decoder_typecast_t)decode_select_input, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_select_input( + labcomm_encoder_t *e, + orca_messages_select_input *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_input); + { + labcomm_encode_int(e, (*v).port); + labcomm_encode_int(e, (*v).decimation); + labcomm_encode_int(e, (*v).list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).list.a[i_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_orca_messages_select_input( +labcomm_encoder_t *e, +orca_messages_select_input *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_input, v); +} +void labcomm_encoder_register_orca_messages_select_input( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_orca_messages_select_input, + (labcomm_encode_typecast_t)encode_select_input + ); +} +int labcomm_sizeof_orca_messages_select_input(orca_messages_select_input *v) +{ + int result = 4; + result += 4 * (*v).list.n_0; + result += 8; + return result; +} +static void decode_select_output( + labcomm_decoder_t *d, + void (*handle)( + orca_messages_select_output *v, + void *context + ), + void *context +) +{ + orca_messages_select_output v; + v.port = labcomm_decode_int(d); + v.decimation = labcomm_decode_int(d); + v.list.n_0 = labcomm_decode_int(d); + v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.list.a[i_0] = labcomm_decode_int(d); + } + } + handle(&v, context); + { + free(v.list.a); + } +} +void labcomm_decoder_register_orca_messages_select_output( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_output *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_orca_messages_select_output, + (labcomm_decoder_typecast_t)decode_select_output, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_select_output( + labcomm_encoder_t *e, + orca_messages_select_output *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_output); + { + labcomm_encode_int(e, (*v).port); + labcomm_encode_int(e, (*v).decimation); + labcomm_encode_int(e, (*v).list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).list.a[i_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_orca_messages_select_output( +labcomm_encoder_t *e, +orca_messages_select_output *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_output, v); +} +void labcomm_encoder_register_orca_messages_select_output( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_orca_messages_select_output, + (labcomm_encode_typecast_t)encode_select_output + ); +} +int labcomm_sizeof_orca_messages_select_output(orca_messages_select_output *v) +{ + int result = 4; + result += 4 * (*v).list.n_0; + result += 8; + return result; +} +static void decode_select_parameter( + labcomm_decoder_t *d, + void (*handle)( + orca_messages_select_parameter *v, + void *context + ), + void *context +) +{ + orca_messages_select_parameter v; + v.port = labcomm_decode_int(d); + v.decimation = labcomm_decode_int(d); + v.list.n_0 = labcomm_decode_int(d); + v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.list.a[i_0] = labcomm_decode_int(d); + } + } + handle(&v, context); + { + free(v.list.a); + } +} +void labcomm_decoder_register_orca_messages_select_parameter( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_parameter *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_orca_messages_select_parameter, + (labcomm_decoder_typecast_t)decode_select_parameter, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_select_parameter( + labcomm_encoder_t *e, + orca_messages_select_parameter *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_parameter); + { + labcomm_encode_int(e, (*v).port); + labcomm_encode_int(e, (*v).decimation); + labcomm_encode_int(e, (*v).list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).list.a[i_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_orca_messages_select_parameter( +labcomm_encoder_t *e, +orca_messages_select_parameter *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_parameter, v); +} +void labcomm_encoder_register_orca_messages_select_parameter( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_orca_messages_select_parameter, + (labcomm_encode_typecast_t)encode_select_parameter + ); +} +int labcomm_sizeof_orca_messages_select_parameter(orca_messages_select_parameter *v) +{ + int result = 4; + result += 4 * (*v).list.n_0; + result += 8; + return result; +} +static void decode_select_log( + labcomm_decoder_t *d, + void (*handle)( + orca_messages_select_log *v, + void *context + ), + void *context +) +{ + orca_messages_select_log v; + v.port = labcomm_decode_int(d); + v.decimation = labcomm_decode_int(d); + v.list.n_0 = labcomm_decode_int(d); + v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + v.list.a[i_0] = labcomm_decode_int(d); + } + } + handle(&v, context); + { + free(v.list.a); + } +} +void labcomm_decoder_register_orca_messages_select_log( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_log *v, + void *context + ), + void *context +) +{ + labcomm_internal_decoder_register( + d, + &labcomm_signature_orca_messages_select_log, + (labcomm_decoder_typecast_t)decode_select_log, + (labcomm_handler_typecast_t)handler, + context + ); +} +static void encode_select_log( + labcomm_encoder_t *e, + orca_messages_select_log *v +) +{ + e->writer.write(&e->writer, labcomm_writer_start); + labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_log); + { + labcomm_encode_int(e, (*v).port); + labcomm_encode_int(e, (*v).decimation); + labcomm_encode_int(e, (*v).list.n_0); + { + int i_0_0; + for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) { + int i_0 = i_0_0; + labcomm_encode_int(e, (*v).list.a[i_0]); + } + } + } + e->writer.write(&e->writer, labcomm_writer_end); +} +void labcomm_encode_orca_messages_select_log( +labcomm_encoder_t *e, +orca_messages_select_log *v +) +{ +labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_log, v); +} +void labcomm_encoder_register_orca_messages_select_log( + struct labcomm_encoder *e +) +{ + labcomm_internal_encoder_register( + e, + &labcomm_signature_orca_messages_select_log, + (labcomm_encode_typecast_t)encode_select_log + ); +} +int labcomm_sizeof_orca_messages_select_log(orca_messages_select_log *v) +{ + int result = 4; + result += 4 * (*v).list.n_0; + result += 8; + return result; +} diff --git a/cpp/orca_messages.h b/cpp/orca_messages.h new file mode 100644 index 0000000000000000000000000000000000000000..7bf38e08b698f96e0a8101666c7780b59f24678e --- /dev/null +++ b/cpp/orca_messages.h @@ -0,0 +1,161 @@ +/* LabComm declarations: +typedef struct { + int index; + string name; + byte signature[_]; +} connection_list_t[_]; +typedef struct { + int port; + int decimation; + int list[_]; +} select_t; +sample struct { + connection_list_t input; + connection_list_t output; + connection_list_t parameter; + connection_list_t log; +} directory; +sample select_t select_input; +sample select_t select_output; +sample select_t select_parameter; +sample select_t select_log; +*/ + + +#ifndef __LABCOMM_orca_messages_H__ +#define __LABCOMM_orca_messages_H__ + +#include "labcomm.h" + +#ifndef PREDEFINED_orca_messages_connection_list_t +typedef struct { + int n_0; + struct { + int index; + char* name; + struct { + int n_0; + unsigned char *a; + } signature; + } *a; +} orca_messages_connection_list_t; +#endif + +#ifndef PREDEFINED_orca_messages_select_t +typedef struct { + int port; + int decimation; + struct { + int n_0; + int *a; + } list; +} orca_messages_select_t; +#endif + +#ifndef PREDEFINED_orca_messages_directory +typedef struct { + orca_messages_connection_list_t input; + orca_messages_connection_list_t output; + orca_messages_connection_list_t parameter; + orca_messages_connection_list_t log; +} orca_messages_directory; +#endif +void labcomm_decoder_register_orca_messages_directory( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_directory *v, + void *context + ), + void *context +); +void labcomm_encoder_register_orca_messages_directory( + struct labcomm_encoder *e); +void labcomm_encode_orca_messages_directory( + struct labcomm_encoder *e, + orca_messages_directory *v +); +extern int labcomm_sizeof_orca_messages_directory(orca_messages_directory *v); + +#ifndef PREDEFINED_orca_messages_select_input +typedef orca_messages_select_t orca_messages_select_input; +#endif +void labcomm_decoder_register_orca_messages_select_input( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_input *v, + void *context + ), + void *context +); +void labcomm_encoder_register_orca_messages_select_input( + struct labcomm_encoder *e); +void labcomm_encode_orca_messages_select_input( + struct labcomm_encoder *e, + orca_messages_select_input *v +); +extern int labcomm_sizeof_orca_messages_select_input(orca_messages_select_input *v); + +#ifndef PREDEFINED_orca_messages_select_output +typedef orca_messages_select_t orca_messages_select_output; +#endif +void labcomm_decoder_register_orca_messages_select_output( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_output *v, + void *context + ), + void *context +); +void labcomm_encoder_register_orca_messages_select_output( + struct labcomm_encoder *e); +void labcomm_encode_orca_messages_select_output( + struct labcomm_encoder *e, + orca_messages_select_output *v +); +extern int labcomm_sizeof_orca_messages_select_output(orca_messages_select_output *v); + +#ifndef PREDEFINED_orca_messages_select_parameter +typedef orca_messages_select_t orca_messages_select_parameter; +#endif +void labcomm_decoder_register_orca_messages_select_parameter( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_parameter *v, + void *context + ), + void *context +); +void labcomm_encoder_register_orca_messages_select_parameter( + struct labcomm_encoder *e); +void labcomm_encode_orca_messages_select_parameter( + struct labcomm_encoder *e, + orca_messages_select_parameter *v +); +extern int labcomm_sizeof_orca_messages_select_parameter(orca_messages_select_parameter *v); + +#ifndef PREDEFINED_orca_messages_select_log +typedef orca_messages_select_t orca_messages_select_log; +#endif +void labcomm_decoder_register_orca_messages_select_log( + struct labcomm_decoder *d, + void (*handler)( + orca_messages_select_log *v, + void *context + ), + void *context +); +void labcomm_encoder_register_orca_messages_select_log( + struct labcomm_encoder *e); +void labcomm_encode_orca_messages_select_log( + struct labcomm_encoder *e, + orca_messages_select_log *v +); +extern int labcomm_sizeof_orca_messages_select_log(orca_messages_select_log *v); + +#define LABCOMM_FORALL_SAMPLES_orca_messages(func, sep) \ + func(directory, orca_messages_directory) sep \ + func(select_input, orca_messages_select_input) sep \ + func(select_output, orca_messages_select_output) sep \ + func(select_parameter, orca_messages_select_parameter) sep \ + func(select_log, orca_messages_select_log) +#endif diff --git a/cpp/record_ya.cc b/cpp/record_ya.cc new file mode 100644 index 0000000000000000000000000000000000000000..95680af540b7fdf1ab8ee43f9b8df6d3a951da68 --- /dev/null +++ b/cpp/record_ya.cc @@ -0,0 +1,61 @@ +#include "record_ya.h" + +void record_ya(const mat& ya,vector<Dmp>& dmpVec,const double& dt,const double& sparse_dt,const int& takeEvery,const int& RECORD_DMP, const int& PLAY_DMP) { + + + static int sample = 0; + static const int dof = 14; + static int recordedBefore = 0; + static mat demoTraj = zeros<mat>(1,dof); + static int stillCount = 0; + static int sampleRecordTot = 0; + + if (!armMoving(ya) && RECORD_DMP) { + ++stillCount; + } else if (armMoving(ya)) { + stillCount = 0; + } + + // Stop/Start recording condition: + int record_dmp = ((RECORD_DMP) && stillCount < 250 && sampleRecordTot > 500) || (PLAY_DMP && RECORD_DMP); + + + if (record_dmp && !recordedBefore) { + + for (int i = 0 ; i < dof ; i++) { + demoTraj(0,i) = ya(i); + } + } + + if (record_dmp) { + ++sample; + if (sample == takeEvery) { + mat tempMat = zeros<mat>(1,dof); + for (int i = 0 ; i < dof ; i++) { + tempMat(0,i) = ya(i); + } + demoTraj = join_vert(demoTraj, tempMat); + sample = 0; + } + } + + + + if (!record_dmp && recordedBefore) { + //cout << "Running traj2dmp" << endl; + dmpVec.push_back(traj2dmp(demoTraj, sparse_dt).speedupTimes(1.0)); + int maxDmpVec = dmpVec.size(); + cout << "We have 1..." << maxDmpVec << " dmp:s" << endl; + demoTraj = zeros<mat>(1,dof); + //cout << dmpVec.back() << endl; + } + recordedBefore = record_dmp; + if (RECORD_DMP) { + ++sampleRecordTot; + } else { + sampleRecordTot = 0; + } + + + +} diff --git a/cpp/record_ya.h b/cpp/record_ya.h new file mode 100644 index 0000000000000000000000000000000000000000..f84a694dd0012ff4b1ed4cc480c4512bc81ff1a2 --- /dev/null +++ b/cpp/record_ya.h @@ -0,0 +1,19 @@ +#include <iostream> +#include <armadillo> +#include "dmp.h" +#include "armmoving.h" +#include "traj2dmp.h" + + + +using namespace arma; +using namespace std; + +#ifndef RECORD_YA_H +#define RECORD_YA_H + + + +void record_ya(const mat& ya,vector<Dmp>& dmpVec,const double& dt,const double& sparse_dt,const int& takeEvery,const int& RECORD_DMPm, const int& PLAY_DMP); + +#endif diff --git a/cpp/state2yardd.cc b/cpp/state2yardd.cc new file mode 100644 index 0000000000000000000000000000000000000000..463b32423fc28c20297cae5bd81dc1014a868369 --- /dev/null +++ b/cpp/state2yardd.cc @@ -0,0 +1,121 @@ +#include "state2yardd.h" +#include "twod_controller.h" + + +void state2yardd(const mat& ya, const mat& yad, mat& yardd, int& PLAY_DMP, const int& DMP_NBR, const vector<Dmp>& dmpVec, mat& yclog, mat& ycddlog, mat& ya_minus_yc, mat& yad_minus_ycd, double& e_norm_log, double& xlog, mat& yc_minus_g, mat& zlog) { + + + // declare: + static const int dof = 14; + static mat z(dof,1); + static double x; + static mat w; + static double tau; + static mat g; + static const double offs = pow(10,-10); + static mat y0(dof,1); + static mat yc = zeros<mat>(dof,1); + static double dt = 0.004; + static const double kc = 10000; + static double x_dot; + static double dmp_nbr_old = DMP_NBR; + static double e = 0; + static double alpha_e = 5; + static const double eps_stop = 0.010; + static const double x_th = 0.10; + mat D = pow((diff(c)*0.55),2); + mat Dlast(1,1); + Dlast = D(D.n_rows - 1); + D = 1/join_vert(D, Dlast); + + + + // Init: + static int play_dmp_old = 0; + if (PLAY_DMP && !play_dmp_old || DMP_NBR != dmp_nbr_old) { + + z.zeros(); + Dmp currentDmp = dmpVec.at(DMP_NBR-1); + w = currentDmp.getW(); + tau = currentDmp.getTau(); + g = currentDmp.getG(); + y0 = ya; + yc = ya; + x = 1; + z.zeros(); + e = 0; + + } + + if (PLAY_DMP) { + double f; + double z_dot; + double e_dot = alpha_e*(norm(ya-yc) - e); + e = e + e_dot*dt; + //cout << e << endl; + double tau_adapt = tau*(1 + kc*pow(e,2)); + //cout << (1 + kc*pow(e,2)) << endl; + mat ycdot(1,dof); + mat ycddot(1,dof); + for (int j = 0; j < dof; ++j) { + mat psi = exp(-0.5*pow(x-c,2)%D); + //f = as_scalar(sum(w.t()*psi)) / as_scalar(sum(psi + offs)) *x * (g(j) - y0(j)); + f = as_scalar(w.col(j).t()*psi) / as_scalar(sum(psi + offs)) * x; + + //f = 0; //////////////////// Test row + z_dot = 1/ tau_adapt*(alpha_z*(beta_z*(g(j)-yc(j)) - z(j)) + f); + z(j) = z(j) + dt * z_dot; + + + + ycdot(j) = z(j)/tau_adapt; + ycddot(j) = (z_dot*tau_adapt-2*tau*kc*z(j)*e*e_dot)/pow(tau_adapt,2); + yc(j) = yc(j) + ycdot(j)*dt; + // Convergence states: + ya_minus_yc(j) = ya(j) - yc(j); + yad_minus_ycd(j) = yad(j) - ycdot(j); + yc_minus_g(j) = yc(j) - g(j); + zlog(j) = z(j); + + } + x_dot = -x/tau_adapt; + x = x + x_dot*dt; + + twod_controller(ya, yad, yc, ycdot, ycddot, yardd); + yclog = yc; + ycddlog = ycddot; + + //cout << norm(ya-g) << endl; + + // Stop at goal: + + if ( (norm(ya-g) < eps_stop) && (x < x_th) ) { + PLAY_DMP = 0; //Optional stopping condition + } + // Convergence states + e_norm_log = e; + xlog = x; + + + } else { + yardd.zeros(); + yclog = yc; + ycddlog.zeros(); + + // Convergence states: + ya_minus_yc.zeros(); + yad_minus_ycd.zeros(); + e_norm_log = 0; + xlog = 0; + yc_minus_g.zeros(); + zlog.zeros(); + + } + + play_dmp_old = PLAY_DMP; + dmp_nbr_old = DMP_NBR; + + + +} + diff --git a/cpp/state2yardd.h b/cpp/state2yardd.h new file mode 100644 index 0000000000000000000000000000000000000000..323cd4a5f3f7907545136242d1b8d43df784040c --- /dev/null +++ b/cpp/state2yardd.h @@ -0,0 +1,19 @@ +#include "traj2dmp.h" +#include <iostream> +#include <armadillo> +#include <math.h> + + + + +using namespace arma; + + +#ifndef STATE2YARDD_H +#define STATE2YARDD_H + + + +void state2yardd(const mat& ya, const mat& yad, mat& yardd, int& PLAY_DMP, const int& DMP_NBR, const vector<Dmp>& dmpVec, mat& yclog, mat& ycddlog, mat& ya_minus_yc, mat& yad_minus_ycd, double& e_norm_log, double& xlog, mat& yc_minus_g, mat& zlog); + +#endif diff --git a/cpp/traj2dmp.cc b/cpp/traj2dmp.cc new file mode 100644 index 0000000000000000000000000000000000000000..6446762a4b04aa8fa2c0c5ee5184296a9de719f5 --- /dev/null +++ b/cpp/traj2dmp.cc @@ -0,0 +1,98 @@ +#include "traj2dmp.h" +#include <iostream> +#include <armadillo> + +using namespace arma; + +mat smartDiff(const mat& inMat) { //now supports col vecs only + mat temp(1,1); + temp << 0 << endr; + mat outMat = (join_vert(temp, diff(inMat)) + join_vert(diff(inMat), temp)) / 2.0; + return outMat; + +} + +Dmp traj2dmp(const mat& traj, const double& sparse_dt) { + + + + mat D = pow((diff(c)*0.55),2); + mat Dlast(1,1); + Dlast = D(D.n_rows - 1); + D = 1/join_vert(D, Dlast); + + + + mat g(1,dof); + mat y0(1,dof); + mat w = zeros<mat>(n_kernel,dof); + + int P = traj.n_rows; + double tau = P*sparse_dt/3; + mat x(P,1); + x(0) = 1; + double x_dot; + for (int t = 1; t < P; ++t) { + x_dot = -x(t-1)/tau; + x(t) = x(t-1) + x_dot*sparse_dt; + } + mat psi(P,n_kernel); + //cout << c.n_rows << endl; + //cout << D.n_rows << endl; + for (int t = 0; t<P; ++t) { + psi.row(t) = exp(-0.5*pow(x(t)-c,2)%D).t(); + } + //cout << psi << endl; + + + + + //cout << psi << endl; + for (int j = 0; j < traj.n_cols; ++j) { + mat T = traj.col(j); + mat TD = smartDiff(T)/sparse_dt; + mat TDD = smartDiff(TD)/sparse_dt; + g(j) = T(T.n_rows-1); + y0(j) = T(0); + + + mat f_target = pow(tau,2)*TDD -alpha_z*(beta_z*(g(j)-T) -tau*TD); + //cout << "f_target:" << endl; + + //cout << f_target << endl; + //mat s = x*(g(j) - y0(j)); + mat s = x; + mat Gamma_i(P,P); + Gamma_i.zeros(); + //cout << psi << endl; + for (int i = 0; i < n_kernel; ++i) { + Gamma_i.diag() = psi.col(i); + //cout << Gamma_i.max() << endl; + //cout << Gamma_i.diag() << endl; + //cout << "Gamma_i: " << endl; + //cout << Gamma_i << endl; + //cout << "psi: " << endl; + //cout << psi.col(i) << endl; + double num = as_scalar(s.t()*Gamma_i*f_target); + double den = as_scalar(s.t()*Gamma_i*s); + + w(i,j) = num / den; + //cout << s.t()*Gamma_i*s << endl; + //cout << den << endl; + + + /*cout << "fit:" << endl; + for (int t = 0; t < P; ++t) { + + double fit = as_scalar(w.col(j).t()*psi.row(t).t()) / as_scalar(sum(psi.row(t) + offs)) * x(t); + cout << fit << endl; + }*/ + //cout << " " << endl; + //cout << " " << endl; + //cout << " " << endl; + } + } + //cout << w << endl; + return Dmp(w, g, tau); +} + diff --git a/cpp/traj2dmp.h b/cpp/traj2dmp.h new file mode 100644 index 0000000000000000000000000000000000000000..918a5f84d5e6932525dcc21a68e66ec246d07807 --- /dev/null +++ b/cpp/traj2dmp.h @@ -0,0 +1,11 @@ +#include "dmp.h" +#include <armadillo> +using namespace arma; +#ifndef TRAJ2DMP_H +#define TRAJ2DMP_H + + + +mat smartDiff(const mat& inMat); +Dmp traj2dmp(const mat& trajectory, const double& dt); +#endif diff --git a/cpp/twod_controller.cc b/cpp/twod_controller.cc new file mode 100644 index 0000000000000000000000000000000000000000..43f91fc03a8742ac81e7f45b076cb6d5abec7162 --- /dev/null +++ b/cpp/twod_controller.cc @@ -0,0 +1,19 @@ +#include "twod_controller.h" + + +void twod_controller(const mat& ya, const mat& yad, const mat& yc, const mat& ycdot, const mat& ycddot, mat& yardd) { + + + //5, 6.25 well balanced for no feed-forward at least + //static const double kv = 10; + //static const double kp = 25; + + //static const double kv = 0; + //static const double kp = 0; + + static const double kv = 10; + static const double kp = kv*kv/4; + + //yardd = kp*(yc - ya) + kv*(ycdot - yad); + yardd = kp*(yc - ya) + kv*(ycdot - yad) + ycddot; +} diff --git a/cpp/twod_controller.h b/cpp/twod_controller.h new file mode 100644 index 0000000000000000000000000000000000000000..4f33fba4c37d163e5f9d79639c84c2bab0105816 --- /dev/null +++ b/cpp/twod_controller.h @@ -0,0 +1,26 @@ +#include <armadillo> +#include <math.h> + + + + +using namespace arma; + + +#ifndef TWOD_CONTROLLER_H +#define TWOD_CONTROLLER_H + + + +void twod_controller(const mat& ya, const mat& yad, const mat& yc, const mat& ycdot, const mat& ycddot, mat& yardd); + +#endif + + + + + + + + + diff --git a/cpp/write_norms_to_file.cc b/cpp/write_norms_to_file.cc new file mode 100644 index 0000000000000000000000000000000000000000..8687328cbc722cace9820bc501ddd4c2aee09b2d --- /dev/null +++ b/cpp/write_norms_to_file.cc @@ -0,0 +1,31 @@ +#include "write_norms_to_file.h" + +void write_norms_to_file(const mat& ya_minus_yc,const mat& yad_minus_ycd,const double& e_norm_log,const double& xlog,const mat& yc_minus_g,const mat& zlog,const int& PLAY_DMP) { + static ofstream logfile; + static int has_played = 0; + + if (PLAY_DMP && !has_played) { + logfile.open("data/logfile1.txt"); + has_played = 1; + } + + if (PLAY_DMP) { + for (int j = 0; j < dof; ++j) { + logfile << ya_minus_yc(j) << " "; + } + for (int j = 0; j < dof; ++j) { + logfile << yad_minus_ycd(j) << " "; + } + logfile << e_norm_log << " "; + logfile << xlog << " "; + for (int j = 0; j < dof; ++j) { + logfile << yc_minus_g(j) << " "; + } + for (int j = 0; j < dof; ++j) { + logfile << zlog(j) << " "; + } + logfile << endl; + } + + +} diff --git a/cpp/write_norms_to_file.h b/cpp/write_norms_to_file.h new file mode 100644 index 0000000000000000000000000000000000000000..ff36d99add6f37c2378f4a7805d6a461514d8ad8 --- /dev/null +++ b/cpp/write_norms_to_file.h @@ -0,0 +1,14 @@ +#include <iostream> +#include <armadillo> +#include <math.h> +#include <fstream> +#include "dmp.h" +using namespace std; +using namespace arma; + +#ifndef WRITE_NORMS_TO_FILE_H +#define WRITE_NORMS_TO_FILE_H + +void write_norms_to_file(const mat& ya_minus_yc,const mat& yad_minus_ycd,const double& e_norm_log,const double& xlog,const mat& yc_minus_g,const mat& zlog, const int& PLAY_DMP); + +#endif