void calc_phase(PHASE_ENCODE_GRADIENT_T *grad, int write_flag, char VJparam_g[], char VJparam_t[])
{
if ((ix > 1) && !sglarray) return;
grad->writeToDisk = write_flag; /* set writeToDiskFlag */
strcpy(grad->param1,VJparam_g); /* assign VNMRJ parameter 1 */
strcpy(grad->param2,VJparam_t); /* assign VNMRJ parameter 2 */
calcPhase(grad); /* calculate and create phase encode grdient */
/* return phase encode gradient parameters to VnmrJ space */
if (strcmp(VJparam_g, ""))
putvalue(VJparam_g, grad->amp);
if (strcmp(VJparam_t, "") )
putvalue(VJparam_t, grad->duration);
}
/***********************************************************************
* Function Name: calc_sim_gradient
* Example: calc_sim_gradient (GENERIC_GRADIENT_T *grad0,
* GENERIC_GRADIENT_T *grad1,
* GENERIC_GRADIENT_T *grad2,
* double min_tpe,
* int write_flag)
* Purpose: Blocks up to three gradient
* Input
* Formal: *grad0 - pointer to generic structure
* *grad0 - pointer to generic structure
* *grad0 - pointer to generic structure
* min_tpe - minimum duration
* write_flag - write to disk flag
* Private: none
* Public:
* Output
* Return: double - duration of blocked greadients
* Formal: none
* Private: none
* Public: none
* Notes: ONLY works for trapezoidal gradients
***********************************************************************/
double calc_sim_gradient(GENERIC_GRADIENT_T *grad0, GENERIC_GRADIENT_T *grad1,
GENERIC_GRADIENT_T *grad2, double min_tpe, int write_flag)
{
int i; /* loop counter */
int typesum = 0; /* check for gradient type */
double time = 0.0; /* longest duration of gradient */
double tramp = 0.0; /* longest ramp time */
double duration0, duration1, duration2; /* gradient durations */
double tramp0, tramp1,tramp2; /* gradient ramp time */
double amp0, amp1, amp2;
GENERIC_GRADIENT_T* grad[3]; /* internal array of structs to allow indexing */
if ((ix > 1) && !sglarray) return(grad0->duration);
/* assign input structs to internal array to allow use in for loop ***/
grad[0] = grad0;
grad[1] = grad1;
grad[2] = grad2;
/* check for shape -> all gradients need to be trapezoidal */
for (i=0; i<3; i++) {
if (grad[i]->shape != TRAPEZOID) {
displayGeneric(grad[i]);
sgl_abort_message("ERROR calc_sim_gradient: Gradient %d ('%s') must be trapezoidal",i,grad[i]->name);
}
}
/* find longest gradient duration */
duration0 = grad[0]->duration - grad[0]->tramp; /* duration of a square gradient with same integral & amplitude */
duration1 = grad[1]->duration - grad[1]->tramp;
duration2 = grad[2]->duration - grad[2]->tramp;
time = MAX(MAX(duration0,duration1),duration2); /* should already be granulated */
if( trampfixed > 0.0 ) {
tramp = grad[0]->tramp;
} else {
amp0 = (fabs(grad[0]->m0) + fabs(grad[0]->areaOffset))/time; /* amplitude of square gradient with same integral and duration "time" */
amp1 = (fabs(grad[1]->m0) + fabs(grad[1]->areaOffset))/time;
amp2 = (fabs(grad[2]->m0) + fabs(grad[2]->areaOffset))/time;
tramp0 = amp0/grad[0]->slewRate; /* ramp time with new amplitude */
tramp1 = amp1/grad[1]->slewRate;
tramp2 = amp2/grad[2]->slewRate;
tramp = MAX(MAX(tramp0,tramp1),tramp2);
tramp = granularity(tramp,grad[0]->resolution);
}
/* But use min_tpe as duration, if it is longer */
time = MAX(time+tramp,granularity(min_tpe,grad[0]->resolution));
/* assign new duration, ramp time, calc flag, and write flag to all 3 */
grad[0]->duration = grad[1]->duration = grad[2]->duration = time;
grad[0]->tramp = grad[1]->tramp = grad[2]->tramp = tramp;
grad[0]->calcFlag = grad[1]->calcFlag = grad[2]->calcFlag = AMPLITUDE_FROM_MOMENT_DURATION_RAMP;
/* set writeToDisk flag */
grad[0]->writeToDisk = grad[1]->writeToDisk = grad[2]->writeToDisk = write_flag;
/* Re-calculate gradients */
for (i=0; i<3; i++) {
switch(grad[i]->type)
{
case REFOCUS_GRADIENT_TYPE:
calcRefocus(grad[i]);
if (strcmp(grad[i]->param1, ""))
putvalue(grad[i]->param1, grad[i]->amp);
break;
case DEPHASE_GRADIENT_TYPE:
calcDephase(grad[i]);
if (strcmp(grad[i]->param1, ""))
putvalue(grad[i]->param1, grad[i]->amp);
break;
case PHASE_GRADIENT_TYPE:
calcPhase(grad[i]);
if (strcmp(grad[i]->param1, ""))
putvalue(grad[i]->param1, grad[i]->amp);
if (strcmp(grad[i]->param2, ""))
putvalue(grad[i]->param2, grad[i]->duration);
break;
case GENERIC_GRADIENT_TYPE:
calcGeneric(grad[i]);
if (strcmp(grad[i]->param1, ""))
putvalue(grad[i]->param1, grad[i]->amp);
break;
default:
typesum += 1;
}
}
if (typesum >=2) {
sgl_abort_message("ERROR calc_sim_gradient: 2 or more unspecified gradients as arguments");
}
/* return longest duration */
return(time);
}
/**
* @brief Given a single demo trajectory, produces a multi-dim DMP
* @param[in] demo An n-dim demonstration trajectory
* @param[in] k_gains A proportional gain for each demo dimension
* @param[in] d_gains A vector of differential gains for each demo dimension
* @param[in] num_bases The number of basis functions to use for the fxn approx (i.e. the order of the Fourier series)
* @param[out] dmp_list An n-dim list of DMPs that are all linked by a single canonical (phase) system
*/
void learnFromDemo(const DMPTraj &demo,
const vector<double> &k_gains,
const vector<double> &d_gains,
const int &num_bases,
vector<DMPData> &dmp_list)
{
//Determine traj length and dim
int n_pts = demo.points.size();
if(n_pts < 1){
ROS_ERROR("Empty trajectory passed to learn_dmp_from_demo service!");
return;
}
int dims = demo.points[0].positions.size();
double tau = demo.times[n_pts-1];
double *x_demo = new double[n_pts];
double *v_demo = new double[n_pts];
double *v_dot_demo = new double[n_pts];
double *f_domain = new double[n_pts];
double *f_targets = new double[n_pts];
FunctionApprox *f_approx = new FourierApprox(num_bases);
//Compute the DMP weights for each DOF separately
for(int d=0; d<dims; d++){
double curr_k = k_gains[d];
double curr_d = d_gains[d];
double x_0 = demo.points[0].positions[d];
double goal = demo.points[n_pts-1].positions[d];
x_demo[0] = demo.points[0].positions[d];
v_demo[0] = 0;
v_dot_demo[0] = 0;
//Calculate the demonstration v and v dot by assuming constant acceleration over a time period
for(int i=1; i<n_pts; i++){
x_demo[i] = demo.points[i].positions[d];
double dx = x_demo[i] - x_demo[i-1];
double dt = demo.times[i] - demo.times[i-1];
v_demo[i] = dx/dt;
v_dot_demo[i] = (v_demo[i] - v_demo[i-1]) / dt;
}
//Calculate the target pairs so we can solve for the weights
for(int i=0; i<n_pts; i++){
double phase = calcPhase(demo.times[i],tau);
f_domain[i] = demo.times[i]/tau; //Scaled time is cleaner than phase for spacing reasons
f_targets[i] = ((tau*tau*v_dot_demo[i] + curr_d*tau*v_demo[i]) / curr_k) - (goal-x_demo[i]) + ((goal-x_0)*phase);
f_targets[i] /= phase; // Do this instead of having fxn approx scale its output based on phase
}
//Solve for weights
f_approx->leastSquaresWeights(f_domain, f_targets, n_pts);
//Create the DMP structures
DMPData *curr_dmp = new DMPData();
curr_dmp->weights = f_approx->getWeights();
curr_dmp->k_gain = curr_k;
curr_dmp->d_gain = curr_d;
dmp_list.push_back(*curr_dmp);
}
delete[] x_demo;
delete[] v_demo;
delete[] v_dot_demo;
delete[] f_domain;
delete[] f_targets;
delete f_approx;
}
void calc_epi(EPI_GRADIENT_T *epi_grad,
READOUT_GRADIENT_T *ro_grad,
PHASE_ENCODE_GRADIENT_T *pe_grad,
REFOCUS_GRADIENT_T *ror_grad,
PHASE_ENCODE_GRADIENT_T *per_grad,
READOUT_GRADIENT_T *nav_grad,
int write_flag) {
/* Variables for generating gradient shapes */
double dwint,blipint,skip,dw;
int np_ramp, np_flat, npro;
double *dwell, *dac;
double pos, neg;
int pt, pts, inx=0, lobe, lobes, blippts, zeropts;
/* Variables for generating tables */
int n, seg, steps;
char order_str[MAXSTR],gpe_tab[MAXSTR],tab_file[MAXSTR];
FILE *fp;
/********************************************************************************/
/* Error Checks */
/********************************************************************************/
/* make sure that the combination of nseg, ky_order and fract_ky make sense */
if (ky_order[0] == 'c') {
/* Can't do just one segment with centric */
if (nseg == 1) {
if (ix == 1) {
warn_message("WARNING %s: ky_order set to 'l'\n",seqfil);
warn_message(" Only linear ordering allowed with single-shot acquisition\n");
}
ky_order[0] = 'l';
}
/* Must have even number of shots with centric ordering */
if (((int) nseg % 2) == 1)
abort_message("ERROR %s: Must do even number of shots with centric ordering\n",seqfil);
/* Can't do fractional ky with centric acquisition */
if (fract_ky != pe_grad->steps/2)
abort_message("ERROR %s: fract_ky must be = nv/2 (%d) for centric acquisition\n",seqfil,(int) (pe_grad->steps/2));
}
if (fract_ky > pe_grad->steps/2)
abort_message("ERROR %s: fract_ky must be <= nv/2 (%d)\n",seqfil,(int) (pe_grad->steps/2));
/* calculate etl */
switch(ky_order[0]) {
case 'l':
epi_grad->etl = (pe_grad->steps/2 + fract_ky)/nseg;
epi_grad->center_echo = fract_ky/nseg - 1;
if (epi_grad->etl - ((int) epi_grad->etl) > 0.005)
abort_message("%s: Echo train length ((%d/2+%d)/%d = %.2f) not an integer\n",
seqfil,(int)pe_grad->steps,(int) fract_ky, (int) nseg,epi_grad->etl);
break;
case 'c':
epi_grad->etl = pe_grad->steps/nseg;
epi_grad->center_echo = 0;
if (epi_grad->etl - ((int) epi_grad->etl) > 0.005)
abort_message("%s: Echo train length (%d/%d) not an integer\n",
seqfil,(int)pe_grad->steps,(int) nseg);
break;
default:
abort_message("%s: ky_order %s not recognized, use 'l' (linear) or 'c' (centric)\n",
seqfil, ky_order);
break;
}
epi_grad->center_echo += ssepi*2;
/********************************************************************************/
/* Generate a single phase encoding blip and the dephaser */
/********************************************************************************/
blipint = 1/(pe_grad->fov/10*nuc_gamma()); /* base step in PE dimension */
switch(ky_order[0]) {
case 'l': /* each blip will jump nseg lines in ky: */
pe_grad->m0 = blipint*nseg;
per_grad->steps = pe_grad->steps; /* each increment is = one blip unit */
per_grad->m0 = blipint*per_grad->steps/2; /* start at nv/2, step 1 */
break;
case 'c': /* each blip will jump nseg/2 lines in ky: */
pe_grad->m0 = blipint*nseg/2;
per_grad->steps = nseg;
per_grad->m0 = blipint*per_grad->steps/2; /* start at nseg/2, step 1 */
break;
default:
abort_message("ky_order %s not recognized, use 'l' (linear) or 'c' (centric)\n",ky_order);
break;
}
/* Phase encoding blip */
pe_grad->calcFlag = SHORTEST_DURATION_FROM_MOMENT;
pe_grad->writeToDisk = FALSE;
calcPhase(pe_grad);
if ((pe_grad->duration < epi_grad->tblip) || (pe_grad->amp/pe_grad->tramp > pe_grad->slewRate)) {
pe_grad->tramp = granularity(MAX(epi_grad->tblip/2,pe_grad->amp/pe_grad->slewRate),pe_grad->resolution);
pe_grad->duration = 2*pe_grad->tramp;
pe_grad->calcFlag = AMPLITUDE_FROM_MOMENT_DURATION_RAMP;
calcPhase(pe_grad);
}
/* Phase encoding dephaser */
per_grad->calcFlag = SHORTEST_DURATION_FROM_MOMENT;
per_grad->maxGrad *= glim;
per_grad->writeToDisk = write_flag;
calcPhase(per_grad);
/* Now adjust the initial dephaser for fractional k-space */
per_grad->amp *= (fract_ky/(pe_grad->steps/2));
/* Create an array for dwell times */
npro = ro_grad->numPointsFreq;
if ((dwell = (double *)malloc(sizeof(double)*npro)) == NULL) {
abort_message("Can't allocate memory for dwell");
}
/********************************************************************************/
/* Generate a single readout lobe */
/********************************************************************************/
ro_grad->writeToDisk = nav_grad->writeToDisk = FALSE;
calcReadout(ro_grad);
dw = granularity(1/sw,1/epi_grad->ddrsr);
dwint = dw*ro_grad->amp;
skip = getval("skip"); /* User specified min delay between acquisitions */
skip = MAX(skip,pe_grad->duration); /* Is PE blip longer? Then use that */
skip /= 2; /* in further calculations, skip is the skipped part on either ramp */
/* If RO ramp - skip isn't at least one dwell, then we can't do ramp sampling */
if (ro_grad->tramp - (skip + dw + getval("aqtm") - at) < dw) {
if (rampsamp[0] == 'y') {
printf("Blip duration or Min echo spacing is long (%.2fus), ramp sampling turned off",2*skip*1e6);
rampsamp[0] = 'n';
}
/* set ramp time and recalculate */
ro_grad->tramp = skip + (dw + getval("aqtm") - at);
calcReadout(ro_grad);
}
/********************************************************************************/
if (rampsamp[0] == 'n') { /* No rampsampling, just use simple RO shape */
/********************************************************************************/
np_ramp = 0;
np_flat = npro;
skip = (ro_grad->atDelayFront + ro_grad->atDelayBack)/2;
/* Set dwell array */
for (pt = 0; pt < npro; pt++)
dwell[pt] = dw;
dwell[npro-1] = 2*dw; /* gradient duration is actually dw too long */
}
/********************************************************************************/
else { /* rampsampling */
/********************************************************************************/
calc_readout_rampsamp(ro_grad,dwell,&skip,&np_ramp);
np_flat = npro - 2*np_ramp;
} /* end if rampsampling = 'y' */
switch (ro_grad->error) {
case ERR_NO_ERROR:
break;
case ERR_AMPLITUDE:
abort_message("Readout gradient too large, increase FOV to %.2fmm",
ro_grad->bandwidth/(ro_grad->gamma * ro_grad->maxGrad * MM_TO_CM));
break;
default:
abort_message("Error in calculation of readout gradient (error %d)",
(int)ro_grad->error);
break;
}
/* We now have a single lobe, keep that for the navigator echo */
nav_grad->amp = ro_grad->amp;
nav_grad->duration = ro_grad->duration;
nav_grad->tramp = ro_grad->tramp;
nav_grad->m0 = ro_grad->m0;
nav_grad->m0ref = ro_grad->m0ref;
nav_grad->dataPoints = ro_grad->dataPoints;
nav_grad->numPoints = ro_grad->numPoints;
nav_grad->slewRate = ro_grad->slewRate;
/********************************************************************************/
/* Readout dephaser */
/********************************************************************************/
ror_grad->balancingMoment0 = nav_grad->m0ref; /* ideal moment */
/* adjust with grora tweaker */
if (grora == 0) {
warn_message("grora tweaker is 0, probably using old protocol - changed to 1.0");
grora = 1.0;
}
ror_grad->balancingMoment0 *= grora;
ror_grad->writeToDisk = write_flag;
calcRefocus(ror_grad);
/********************************************************************************/
/* Expand gradient shapes to full echo train */
/********************************************************************************/
/* Readout - The navigator shape holds a single lobe */
/********************************************************************************/
/* Does positive or negative lobe need to be adjusted for tweaker? */
pos = neg = 1;
if (groa >= 0) /* Adjust negative downwards, since positive is already at max */
neg = (1 - groa/nav_grad->amp);
else /* groa < 0, Adjust positive downwards */
pos = (1 + groa/nav_grad->amp);
/* Increase the size of ro shape to hold full shape */
/* free(ro_grad->dataPoints); Don't free this, it's now assigned to the navigator echo */
lobes = (epi_grad->etl + ssepi*2);
ro_grad->numPoints *= lobes;
if ((ro_grad->dataPoints = (double *)malloc(ro_grad->numPoints*sizeof(double))) == NULL)
abort_message("%s: Problem allocating memory for EPI readout gradient",seqfil);
ro_grad->duration *= lobes;
/* Concatenate positive and negative lobes */
/* until we have a full echo train (plus ssepi) */
pts = nav_grad->numPoints;
inx = 0;
for (lobe = 0; lobe < lobes/2; lobe++) {
for (pt = 0; pt < pts; pt++)
ro_grad->dataPoints[inx++] = nav_grad->dataPoints[pt] * pos;
for (pt = 0; pt < pts; pt++)
ro_grad->dataPoints[inx++] = -nav_grad->dataPoints[pt] * neg;
}
/********************************************************************************/
/* Phase encoding */
/********************************************************************************/
/* Keep blip */
blippts = pe_grad->numPoints;
if ((dac = (double *)malloc(blippts*sizeof(double))) == NULL)
abort_message("%s: Problem allocating memory for EPI blip",seqfil);
for (pt = 0; pt < blippts; pt++)
dac[pt] = pe_grad->dataPoints[pt];
/* Increase the size of pe shape to hold full shape */
free(pe_grad->dataPoints);
if ((pe_grad->dataPoints = (double *)malloc(ro_grad->numPoints*sizeof(double))) == NULL)
abort_message("%s: Problem allocating memory for EPI phase encoding gradient",seqfil);
/* Pad front of shape - including ssepi time - with zeros */
inx = 0;
lobes = ssepi*2;
zeropts = nav_grad->numPoints;
for (lobe = 0; lobe < lobes; lobe++) {
for (pt = 0; pt < zeropts; pt++) {
pe_grad->dataPoints[inx++] = 0;
}
}
zeropts = nav_grad->numPoints - blippts/2;
for (pt = 0; pt < zeropts; pt++)
pe_grad->dataPoints[inx++] = 0;
lobes = epi_grad->etl-1;
zeropts = nav_grad->numPoints - blippts;
for (lobe=0; lobe < lobes; lobe++) {
for (pt = 0; pt < blippts; pt++)
pe_grad->dataPoints[inx++] = dac[pt];
for (pt = 0; pt < zeropts; pt++)
pe_grad->dataPoints[inx++] = 0;
}
/* Add a few zeros, half the duration of the blip + one lobe, at the very end */
zeropts = blippts/2 + nav_grad->numPoints;
zeropts = blippts/2;
for (pt = 0; pt < zeropts; pt++)
pe_grad->dataPoints[inx++] = 0;
pe_grad->numPoints = ro_grad->numPoints;
pe_grad->duration = ro_grad->duration;
/********************************************************************************/
/* Keep some parameters in EPI struct */
/********************************************************************************/
epi_grad->skip = skip;
epi_grad->np_flat = np_flat;
epi_grad->np_ramp = np_ramp;
epi_grad->dwell = dwell;
epi_grad->duration = ro_grad->duration;
epi_grad->numPoints = ro_grad->numPoints;
epi_grad->amppos = nav_grad->amp*pos;
epi_grad->ampneg = -nav_grad->amp*neg;
epi_grad->amppe = pe_grad->amp;
/********************************************************************************/
/* Write shapes to disk */
/********************************************************************************/
if (writeToDisk(ro_grad->dataPoints, ro_grad->numPoints, 0,
ro_grad->resolution, TRUE /* rollout */,
ro_grad->name) != ERR_NO_ERROR)
abort_message("Problem writing shape %s (%d points) to disk",
ro_grad->name,(int) ro_grad->numPoints);
if (writeToDisk(nav_grad->dataPoints, nav_grad->numPoints, 0,
nav_grad->resolution, TRUE /* rollout */,
nav_grad->name) != ERR_NO_ERROR)
abort_message("Problem writing shape %s (%d points) to disk",
nav_grad->name,(int) nav_grad->numPoints);
if (writeToDisk(pe_grad->dataPoints, pe_grad->numPoints, 0,
pe_grad->resolution, TRUE /* rollout */,
pe_grad->name) != ERR_NO_ERROR)
abort_message("Problem writing shape %s (%d points) to disk",
pe_grad->name,(int) pe_grad->numPoints);
/********************************************************************************/
/* Create EPI tables */
/********************************************************************************/
/* Generates two tables: */
/* t1 that specifies the k-space ordering, used by recon_all */
/* t2 that specifies which direction in k-space to go in a given shot */
/* 1 = positive blips, and -1 = negative blips */
if ((epi_grad->table1 = (int *)malloc(pe_grad->steps*sizeof(int))) == NULL)
abort_message("%s: Problem allocating memory for EPI table1",seqfil);
if ((epi_grad->table2 = (int *)malloc(nseg*sizeof(int))) == NULL)
abort_message("%s: Problem allocating memory for EPI table2",seqfil);
switch(ky_order[0]) {
case 'l':
inx = 0;
for (seg = 0; seg < nseg; seg++) {
epi_grad->table1[inx++] = -fract_ky + seg;
for (n = 0; n < epi_grad->etl-1; n++) {
epi_grad->table1[inx] = (int) (epi_grad->table1[inx-1]+nseg);
inx++;
}
}
for (seg = 0; seg < nseg; seg++)
epi_grad->table2[seg] = 1;
break;
case 'c':
inx = 0;
for (seg = 0; seg < nseg; seg++) {
epi_grad->table1[inx++] = -(nseg/2 - seg);
for (n = 0; n < epi_grad->etl-1; n++) {
if (epi_grad->table1[inx-1] >= 0)
epi_grad->table1[inx] = (int) (epi_grad->table1[inx-1] + nseg/2);
else
epi_grad->table1[inx] = (int) (epi_grad->table1[inx-1] - nseg/2);
inx++;
}
}
for (seg = 0; seg < nseg; seg++)
epi_grad->table2[seg] = ((nseg/2 - 1 - seg >= 0) ? -1 : 1);
break;
default: /* This should have been caught earlier */
break;
}
steps = inx;
/* Print table t1 to file in tablib */
switch(ky_order[0]) {
case 'l': sprintf(order_str,"lin"); break;
case 'c': sprintf(order_str,"cen"); break;
default:
abort_message("%s: ky_order %s not recognized, use 'l' (linear) or 'c' (centric)\n",
seqfil,ky_order);
}
if (ky_order[1] == 'r') /* not reversed */
sprintf(gpe_tab,"%s_nv%d_f%d_%s%d_rev",seqfil,
(int)pe_grad->steps,(int)fract_ky,order_str,(int)nseg);
else
sprintf(gpe_tab,"%s_nv%d_f%d_%s%d",seqfil,
(int)pe_grad->steps,(int)fract_ky,order_str,(int)nseg);
sprintf(tab_file,"%s/tablib/%s",userdir,gpe_tab);
if ((fp = fopen(tab_file,"w")) == NULL) {
abort_message("Error opening file %s\n",gpe_tab);
}
fprintf(fp,"t1 = ");
for (inx = 0; inx < steps; inx++) {
if (ky_order[1] == 'r')
fprintf(fp,"%d ", epi_grad->table1[inx]);
else
fprintf(fp,"%d ", -epi_grad->table1[inx]);
}
fprintf(fp,"\n");
fclose(fp);
strcpy(petable,gpe_tab);
putstring("petable",gpe_tab);
/* Return values in VnmrJ parameter pe_table */
putCmd("exists('pe_table','parameter'):$ex\n");
putCmd("if ($ex > 0) then\n");
putCmd(" pe_table = 0\n"); //reset pe_table array
for (inx = 0; inx < steps; inx++) {
putCmd(" pe_table[%d] = %d\n",inx+1,
((ky_order[1] == 'r') ? 1 : -1)*epi_grad->table1[inx]);
}
putCmd("endif\n");
}
/**
* @brief Use the current active multi-dim DMP to create a plan starting from x_0 toward a goal
* @param[in] dmp_list An n-dim list of DMPs that are all linked by a single canonical (phase) system
* @param[in] x_0 The (n-dim) starting state for planning
* @param[in] x_dot_0 The (n-dim) starting instantaneous change in state for planning
* @param[in] t_0 The time in seconds at which to begin the planning segment. Should only be nonzero when doing a partial segment plan that does not start at beginning of DMP
* @param[in] goal The (n-dim) goal point for planning
* @param[in] goal_thresh Planning will continue until system is within the specified threshold of goal in each dimension
* @param[in] seg_length The length of the requested plan segment in seconds. Set to -1 if plan until goal is desired.
* @param[in] tau The time scaling constant (in this implementation, it is the desired length of the TOTAL (not just this segment) DMP execution in seconds)
* @param[in] total_dt The desired time resolution of the plan
* @param[in] integrate_iter The number of loops used when numerically integrating accelerations
* @param[out] plan An n-dim plan starting from x_0
* @param[out] at_goal True if the final time is greater than tau AND the planned position is within goal_thresh of the goal
*/
void generatePlan(const vector<DMPData> &dmp_list,
const vector<double> &x_0,
const vector<double> &x_dot_0,
const double &t_0,
const vector<double> &goal,
const vector<double> &goal_thresh,
const double &seg_length,
const double &tau,
const double &total_dt,
const int &integrate_iter,
DMPTraj &plan,
uint8_t &at_goal)
{
plan.points.clear();
plan.times.clear();
at_goal = false;
int dims = dmp_list.size();
int n_pts = 0;
double dt = total_dt / integrate_iter;
vector<double> *x_vecs, *x_dot_vecs;
vector<double> t_vec;
x_vecs = new vector<double>[dims];
x_dot_vecs = new vector<double>[dims];
FunctionApprox **f = new FunctionApprox*[dims];
for(int i=0; i<dims; i++)
f[i] = new FourierApprox(dmp_list[i].weights);
double t = 0;
double f_eval;
//Plan for at least tau seconds. After that, plan until goal_thresh is satisfied.
//Cut off if plan exceeds MAX_PLAN_LENGTH seconds, in case of overshoot / oscillation
//Only plan for seg_length seconds if specified
bool seg_end = false;
while(((t+t_0) < tau || (!at_goal && t<MAX_PLAN_LENGTH)) && !seg_end){
//Check if we've planned to the segment end yet
if(seg_length > 0){
if (t > seg_length) seg_end = true;
}
//Plan in each dimension
for(int i=0; i<dims; i++){
double x,v;
if(n_pts==0){
x = x_0[i];
v = x_dot_0[i];
}
else{
x = x_vecs[i][n_pts-1];
v = x_dot_vecs[i][n_pts-1] * tau;
}
//Numerically integrate to get new x and v
for(int iter=0; iter<integrate_iter; iter++)
{
//Compute the phase and the log of the phase to assist with some numerical issues
//Then, evaluate the function approximator at the log of the phase
double s = calcPhase((t+t_0) + (dt*iter), tau);
double log_s = (t+t_0)/tau;
if(log_s >= 1.0){
f_eval = 0;
}
else{
f_eval = f[i]->evalAt(log_s) * s;
}
//Update v dot and x dot based on DMP differential equations
double v_dot = (dmp_list[i].k_gain*((goal[i]-x) - (goal[i]-x_0[i])*s + f_eval) - dmp_list[i].d_gain*v) / tau;
double x_dot = v/tau;
//Update state variables
v += v_dot * dt;
x += x_dot * dt;
}
//Add current state to the plan
x_vecs[i].push_back(x);
x_dot_vecs[i].push_back(v/tau);
}
t += total_dt;
t_vec.push_back(t);
n_pts++;
//If plan is at least minimum length, check to see if we are close enough to goal
if((t+t_0) >= tau){
at_goal = true;
for(int i=0; i<dims; i++){
if(goal_thresh[i] > 0){
if(fabs(x_vecs[i][n_pts-1] - goal[i]) > goal_thresh[i])
at_goal = false;
}
}
}
}
//Create a plan from the generated trajectories
plan.points.resize(n_pts);
for(int j=0; j<n_pts; j++){
plan.points[j].positions.resize(dims);
plan.points[j].velocities.resize(dims);
}
for(int i=0; i<dims; i++){
for(int j=0; j<n_pts; j++){
plan.points[j].positions[i] = x_vecs[i][j];
plan.points[j].velocities[i] = x_dot_vecs[i][j];
}
}
plan.times = t_vec;
//Clean up
for(int i=0; i<dims; i++){
delete f[i];
}
delete[] f;
delete[] x_vecs;
delete[] x_dot_vecs;
}
