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; }