dmpTask::~dmpTask() {
int i;
if (vnames_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) vnames_CBi[i]);
free((void *) vnames_CBi);
}
if (units_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) units_CBi[i]);
free((void *) units_CBi);
}
if (CBi_traj != NULL)
my_free_matrix(CBi_traj, 1, n_CBi, 1, dof_CBi);
if (vnames_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) vnames_Kinect[i]);
free((void *) vnames_Kinect);
}
if (units_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) units_Kinect[i]);
free((void *) units_Kinect);
}
if (Kinect_traj != NULL)
my_free_matrix(Kinect_traj, 1, n_Kinect, 1, dof_Kinect);
if (DMP_object != NULL)
delete DMP_object;
}
static int my_set_image(t_xvar *xvar, t_object *object, t_object *light)
{
t_matrix *scn;
t_k *k;
int x;
int y;
if ((scn = my_new_matrix(4, 1)) == NULL || light == NULL)
return (1);
y = 0;
while (y < xvar->y)
{
x = 0;
while (x < xvar->x)
{
my_help_set_image(xvar, scn, x, y);
if ((k = my_found_inter(object, scn)) == NULL)
return (1);
if (k->k > 0)
my_put_p_i(xvar->img, &k->object->color, (y * xvar->x + x) * 4);
my_free_k(k);
x++;
}
y++;
}
my_free_matrix(scn);
return (0);
}
BaseStateEstimation::~BaseStateEstimation()
{
my_free_matrix(I_rot_B_,1,3,1,3);
my_free_vector(I_linpos_B_,1,3);
my_free_matrix(O_rot_I_,1,3,1,3);
my_free_vector(O_angrate_I_,1,3);
my_free_vector(O_linacc_I_,1,3);
my_free_vector(O_angacc_I_,1,3);
my_free_vector(O_linvel_I_,1,3);
my_free_vector(O_linpos_I_,1,3);
my_free_matrix(O_rot_B_,1,3,1,3);
my_free_vector(O_linacc_B_,1,3);
my_free_vector(O_linvel_B_,1,3);
my_free_vector(O_linpos_B_,1,3);
my_free_vector(prev_O_angrate_I_,1,3);
// my_free_vector(gravity_,1,3);
}
static void my_free_k(t_k *k)
{
my_free_matrix(k->inter);
my_free_matrix(k->inter_nor);
free(k);
}
int dmpTask::initialize()
{
double freq;
int i;
int ans = 999;
start_time_ = 0.0;
// Make sure that everything is clear
if (vnames_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) vnames_CBi[i]);
free((void *) vnames_CBi);
vnames_CBi = NULL;
}
if (units_CBi != NULL)
{
for (i = 1; i <= dof_CBi; i++)
free((void *) units_CBi[i]);
free((void *) units_CBi);
units_CBi = NULL;
}
if (CBi_traj != NULL)
{
my_free_matrix(CBi_traj, 1, n_CBi, 1, dof_CBi);
CBi_traj = NULL;
}
if (vnames_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) vnames_Kinect[i]);
free((void *) vnames_Kinect);
vnames_Kinect = NULL;
}
vnames_Kinect = NULL;
if (units_Kinect != NULL)
{
for (i = 1; i <= dof_Kinect; i++)
free((void *) units_Kinect[i]);
free((void *) units_Kinect);
units_Kinect = NULL;
}
if (Kinect_traj != NULL)
{
my_free_matrix(Kinect_traj, 1, n_Kinect, 1, dof_Kinect);
Kinect_traj = NULL;
}
if (DMP_object != NULL)
{
delete DMP_object;
DMP_object = NULL;
}
// Read robot trajectory
// mrdplot_convert(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi);
mrdplot_read(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi);
if (!set_active_dofs(vnames_CBi, dof_CBi, active_dofs))
return FALSE;
printf("%d active DoFs:", active_dofs[0]);
for (i = 2; i <= active_dofs[0]+1; i++)
{
if (!((i-2)%8))
printf("\n");
printf("%s, ", vnames_CBi[i]);
}
printf("\n\n");
// Read Kinect trajectory
// mrdplot_convert(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect);
mrdplot_read(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect);
// Filter Kinect trajectory if desired
if (Kinect_traj != NULL && filter_Kinect_data)
{
if (!init_filters())
return FALSE;
for (int i = 1; i <= 19; i++)
fth[i].cutoff = 9;
for (int i = 1; i <= 19; i++)
{
fth[i].raw[0] = fth[i].raw[1] = fth[i].raw[2] = Kinect_traj[1][i+1];
fth[i].filt[0] = fth[i].filt[1] = fth[i].filt[2] = Kinect_traj[1][i+1];
}
for (int j = 1; j <= n_Kinect; j++)
for (int i = 1; i <= 19; i++)
Kinect_traj[j][i+1] = filt(Kinect_traj[j][i+1], &fth[i]);
}
// Compute DMP for robot trajectory
if (DMP_object != NULL)
delete DMP_object;
// DMP_object = new DMP_structure(DMP_file);
DMP_object = new DMP_structure(active_dofs[0], 50, 2.0, 12.0, 3.0);
printf("\n");
if (!DMP_object->example_Matrix(CBi_traj, n_CBi, dof_CBi))
return FALSE;
DMP_object->DMP_estimate(0);
// DMP_object->DMP_param_print();
printf("\n");
// Initialize DMP integration
for (int i = 1; i <= active_dofs[0]; i++)
{
initial_positions[i] = CBi_traj[1][i+1];
initial_velocities[i] = 0.0;
}
DMP_object->set_initial_DMP_state(&(initial_positions[1]), &(initial_velocities[1]));
// include this file to define contact points (needed to compute center of pressure)
#include "LEKin_contact.h"
// prepare going to the default posture
bzero((char *)&(target_[1]),N_DOFS*sizeof(target_[1]));
for (i = 1; i <= N_DOFS; i++)
target_[i] = joint_default_state[i];
target_[L_EB].th = target_[R_EB].th = 0.05;
target_[B_TFE].th = 0.2;
for (int i = 1; i <= active_dofs[0]; i++)
{
target_[active_dofs[i]].th = initial_positions[i];
target_[active_dofs[i]].thd = 0.0;
target_[active_dofs[i]].thdd = 0.0;
target_[active_dofs[i]].uff = 0.0;
}
bool there = true;
for (int i = 1; i <= B_HR; i++)
if (fabs(target_[i].th - joint_des_state[i].th) > 1.0e-3)
{
there = false;
break;
}
// go to the target using inverse dynamics (ID)int SampleTask::changeParameters()
if (!there)
{
if (!go_target_wait_ID(target_))
{
return FALSE;
}
}
// ready to go
while (ans == 999)
{
if (!get_int(const_cast<char*>("Enter 1 to start or anthing else to abort ..."), ans, &ans))
{
return FALSE;
}
}
// only go when user really types the right thing
if (ans != 1)
{
return FALSE;
}
start_time_ = task_servo_time;
printf("start time = %.3f, task_servo_time = %.3f\n", start_time_, task_servo_time);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note parm_opt
\date 10/20/91
\remarks
this is the major optimzation program
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] tol : error tolernance to be achieved
\param[in] n_parm : number of parameters to be optimzed
\param[in] n_con : number of contraints to be taken into account
\param[in] f_dLda : function which calculates the derivative of the
optimziation criterion with respect to the parameters;
must return vector
\param[in] f_dMda : function which calculates the derivate of the
constraints with respect to parameters
must return matrix
\param[in] f_M : constraints function, must always be formulted to
return 0 for properly fulfilled constraints
\param[in] f_L : function to calculate simple cost (i.e., constraint
cost NOT included), the constraint costs are added
by this program automatically, the function returns
a double scalar
\param[in] f_dLdada : second derivative of L with respect to the parameters,
must be a matrix of dim n_parm x n_parm
\param[in] f_dMdada : second derivative of M with respect to parameters,
must be a matrix n_con*n_parm x n_parm
\param[in] use_newton: TRUE or FALSE to indicate that second derivatives
are given and can be used for Newton algorithm
\param[in,out] a : initial setting of parameters and also return of
optimal value (must be a vector, even if scalar)
\param[out] final_cost: the final cost
\param[out] err : the sqrt of the squared error of all constraints
NOTE: - program returns TRUE if everything correct, otherwise FALSE
- always minimizes the cost!!!
- algorithms come from Dyer McReynolds
NOTE: besides the possiblity of a bug, the Newton method seems to
sacrifice the validity of the constraint a little up to
quite a bit and should be used prudently
******************************************************************************/
int
parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(),
void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(),
void (*f_dLdada)(), int use_newton, double *final_cost, double *err)
{
register int i,j,n;
double cost= 999.e30;
double last_cost = 0.0;
double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */
double **dMda=NULL, **dMda_t=NULL;
double *dLda;
double *K=NULL; /* the error in the constraints */
double eps = 0.025; /* the learning rate */
double **aux_mat=NULL; /* needed for inversion of matrix */
double *aux_vec=NULL;
double *new_a;
double **dMdada=NULL;
double **dLdada=NULL;
double **A=NULL; /* big matrix, a combination of several other matrices */
double *B=NULL; /* a big vector */
double **A_inv=NULL;
int rc=TRUE;
long count = 0;
int last_sign = 1;
int pending1 = FALSE, pending2 = FALSE;
int firsttime = TRUE;
int newton_active = FALSE;
dLda = my_vector(1,n_parm);
new_a = my_vector(1,n_parm);
if (n_con > 0) {
mult = my_vector(1,n_con);
dMda = my_matrix(1,n_con,1,n_parm);
dMda_t = my_matrix(1,n_parm,1,n_con);
K = my_vector(1,n_con);
aux_mat = my_matrix(1,n_con,1,n_con);
aux_vec = my_vector(1,n_con);
}
if (use_newton) {
dLdada = my_matrix(1,n_parm,1,n_parm);
A = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
A_inv = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
B = my_vector(1,n_parm+n_con);
if (n_con > 0) {
dMdada = my_matrix(1,n_con*n_parm,1,n_parm);
new_mult = my_vector(1,n_con);
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1+n_parm; j<=n_con+n_parm; ++j) {
A[i][j] = 0.0;
}
}
}
while (fabs(cost-last_cost) > *tol) {
++count;
pending1 = FALSE;
pending2 = FALSE;
AGAIN:
/* calculate the current Lagrange multipliers */
if (n_con > 0) {
(*f_M)(a,K); /* takes the parameters, returns residuals */
(*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */
}
(*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */
if (n_con > 0) {
mat_trans(dMda,dMda_t);
}
if (newton_active) {
if (n_con > 0) {
(*f_dMdada)(a,dMdada);
}
(*f_dLdada)(a,dLdada);
}
/* the first step is always a gradient step */
if (newton_active) {
if (firsttime) {
firsttime = FALSE;
eps = 0.1;
}
/* build the A matrix */
for (i=1; i<=n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[i][j] = dLdada[i][j];
for (n=1; n<=n_con; ++n) {
A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j];
}
}
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[j][i] = A[i][j] = dMda[i-n_parm][j];
}
}
/* build the B vector */
if (n_con > 0) {
mat_vec_mult(dMda_t,mult,B);
}
for (i=1; i<=n_con; ++i) {
B[i+n_parm] = K[i];
}
/* invert the A matrix */
if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) {
rc = FALSE;
break;
}
mat_vec_mult(A_inv,B,B);
vec_mult_scalar(B,eps,B);
for (i=1; i<=n_parm; ++i) {
new_a[i] = a[i] + B[i];
}
for (i=1; i<=n_con; ++i) {
new_mult[i] = mult[i] + B[n_parm+i];
}
} else {
if (n_con > 0) {
/* the mulitpliers are updated according:
mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t) */
mat_mult(dMda,dMda_t,aux_mat);
if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) {
rc = FALSE;
break;
}
mat_vec_mult(dMda,dLda,aux_vec);
vec_mult_scalar(K,1./eps,K);
vec_sub(K,aux_vec,aux_vec);
mat_vec_mult(aux_mat,aux_vec,mult);
}
/* the update step looks the following:
a_new = a - eps * (dLda + mult_t * dMda)_t */
if (n_con > 0) {
vec_mat_mult(mult,dMda,new_a);
vec_add(dLda,new_a,new_a);
} else {
vec_equal(dLda,new_a);
}
vec_mult_scalar(new_a,eps,new_a);
vec_sub(a,new_a,new_a);
}
if (count == 1 && !pending1) {
last_cost = (*f_L)(a);
if (n_con > 0) {
(*f_M)(a,K);
last_cost += vec_mult_inner(K,mult);
}
} else {
last_cost = cost;
}
/* calculate the updated cost */
cost = (*f_L)(new_a);
/*printf(" %f\n",cost);*/
if (n_con > 0) {
(*f_M)(new_a,K);
if (newton_active) {
cost += vec_mult_inner(K,new_mult);
} else {
cost += vec_mult_inner(K,mult);
}
}
/* printf("last=%f new=%f\n",last_cost,cost); */
/* check out whether we reduced the cost */
if (cost > last_cost && fabs(cost-last_cost) > *tol) {
/* reduce the gradient climbing rate: sometimes a reduction of eps
causes an increase in cost, thus leave an option to increase
eps */
cost = last_cost; /* reset last_cost */
if (pending1 && pending2) {
/* this means that either increase nor decrease
of eps helps, ==> leave the program */
rc = TRUE;
break;
} else if (pending1) {
eps *= 4.0; /* the last cutting by half did not help, thus
multiply by 2 to get to previous value, and
one more time by 2 to get new value */
pending2 = TRUE;
} else {
eps /= 2.0;
pending1 = TRUE;
}
goto AGAIN;
} else {
vec_equal(new_a,a);
if (newton_active && n_con > 0) {
vec_equal(new_mult,mult);
}
if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD)
newton_active = TRUE;
}
}
my_free_vector(dLda,1,n_parm);
my_free_vector(new_a,1,n_parm);
if (n_con > 0) {
my_free_vector(mult,1,n_con);
my_free_matrix(dMda,1,n_con,1,n_parm);
my_free_matrix(dMda_t,1,n_parm,1,n_con);
my_free_vector(K,1,n_con);
my_free_matrix(aux_mat,1,n_con,1,n_con);
my_free_vector(aux_vec,1,n_con);
}
if (use_newton) {
my_free_matrix(dLdada,1,n_parm,1,n_parm);
my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con);
my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con);
my_free_vector(B,1,n_parm+n_con);
if (n_con > 0) {
my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm);
my_free_vector(new_mult,1,n_con);
}
}
*final_cost = cost;
*tol = fabs(cost-last_cost);
if (n_con > 0) {
*err = sqrt(vec_mult_inner(K,K));
} else {
*err = 0.0;
}
/*
printf("count=%ld rc=%d\n",count,rc);
*/
return rc;
}
/*!*****************************************************************************
*******************************************************************************
\note read_traj_file
\date June 1999
\remarks
parse a script which describes the traj task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] flag : true= use desired data, false use actual data
******************************************************************************/
static int read_traj_file() {
int j,i,r,k,rc;
static char string[100];
static char fname[100] = "traj_strike.txt";
FILE * fp = NULL;
int found = FALSE;
Matrix buff;
int aux;
int ans = 0;
double data = 0.0;
/* open the file, and parse the parameters */
while (TRUE) {
if (!get_string("Name of the Traj File\0",fname,fname))
return FALSE;
/* try to read this file */
sprintf(string, "%s/%s", "saveData", fname);
/* try to count the number of lines */
traj_len = count_traj_file(string);
get_int("What percentage of trajectory do you want to keep?", 100, &ans);
traj_len = (int)(traj_len * (double)ans / 100.0);
printf("Keeping the first %d indices...\n", traj_len);
//printf("%d\n", traj_len);
if (traj_len == -1) { // problem
return FALSE;
}
fp = fopen(string,"r");
if (fp != NULL) {
found = TRUE;
break;
}
else {
printf("ERROR: Could not open file >%s<\n", string);
}
}
/* get the number of rows, columns, sampling frequency
and calc the buffer_size */
//rc = fscanf(fp, "%d %d %d %lf", &buffer_size, &N_COLS, &traj_len, &SAMP_FREQ);
/* read file into a buffer and check if the matrix size is correct */
buff = my_matrix(1,traj_len,1,N_COLS);
q0 = my_vector(1,N_DOFS);
int out = 0;
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_COLS; k++) {
out = fscanf(fp, "%lf", &data);
buff[r][k] = data;
}
}
fclose(fp);
// print buffs first 10 elements
//print_mat("Buffer:\n", buff);
//print_mat_size("Buffer:\n", buff, 10, N_COLS);
/* create the pos, vel, acc , uff matrices that define the trajectory */
traj_pos = my_matrix(1, traj_len, 1, N_DOFS);
traj_vel = my_matrix(1, traj_len, 1, N_DOFS);
traj_acc = my_matrix(1, traj_len, 1, N_DOFS);
traj_uff = my_matrix(1, traj_len, 1, N_DOFS);
// save q0
for (j = 1; j <= N_DOFS; j++)
q0[j] = buff[1][2*j];
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_DOFS; k++) {
traj_pos[r][k] = buff[r][2*k];
traj_vel[r][k] = buff[r][2*k+1];
}
}
//print_mat_size("Vel:\n", traj_vel, 10, n_dofs);
// numerically differentiate traj_vel instead!
num_diff(traj_acc, traj_vel, SAMP_FREQ);
/* free up memory by deallocating resources */
my_free_matrix(buff,1,traj_len,1,N_COLS);
return found;
}
/*!*****************************************************************************
*******************************************************************************
\note monitor_min_max
\date Dec. 2000
\remarks
monitors the min & max values of all joints continuously until keyboard
is hit. Then, an appropriate min_max structure is printed out for usuage
in SensorOffsets.cf
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
monitor_min_max(void)
{
int i,j;
Matrix minmax;
int n_bytes;
double offset = 0.05;
if (!servo_enabled) {
beep(1);
printf("ERROR: motor servo is not running!!\n");
return;
}
minmax = my_matrix(1,n_dofs,MIN_THETA,MAX_THETA);
for (i=1; i<=n_dofs; ++i) {
minmax[i][MIN_THETA] = 1.e10;
minmax[i][MAX_THETA] = -1.e10;
}
printf("Hit any key to stop monitoring .....");
fflush(stdout);
/* just check for min & max values */
while (TRUE) {
for (i=1; i<=n_dofs; ++i) {
/* update min/max values */
if (joint_state[i].th > minmax[i][MAX_THETA])
minmax[i][MAX_THETA] = joint_state[i].th;
if (joint_state[i].th < minmax[i][MIN_THETA])
minmax[i][MIN_THETA] = joint_state[i].th;
}
/* check whether the keyboard was hit */
#ifdef VX
ioctl(0 , FIONREAD, (int) (&n_bytes));
#else
ioctl( 0, FIONREAD, (void *) (&n_bytes) );
#endif
if (n_bytes != 0)
break;
taskDelay(ns2ticks(10000000)); // wait 10ms
}
getchar();
get_double("Safety margin from joint stop?",offset,&offset);
/* print the min/max values in SensorOffset.cf format */
for (i=1; i<=n_dofs; ++i) {
printf("%10s %f %f %f %f %f %f\n",
joint_names[i],
minmax[i][MIN_THETA]+offset,
minmax[i][MAX_THETA]-offset,
joint_default_state[i].th,
joint_opt_state[i].th,
joint_opt_state[i].w,
joint_range[i][THETA_OFFSET]);
}
printf("\n");
fflush(stdout);
my_free_matrix(minmax,1,n_dofs,MIN_THETA,MAX_THETA);
}
