问轨迹优化

EN

for position in sref_
    ... // get position data
    // start parallel region
    for time in vtwin->trefloc_                    // time will be offsetted
        for start_velocity in vtwin->vrefloc_      // start_velocity will be offsetted
        ... // check constraint
            for stop_velocity in vtwin->vrefloc_   // stop_velocity will be offsetted
            ... // check many constraints
            ... // temporarily save 6 values if better than of other stop_velocity
        ... // save the 6 values which proofed to be best to output
    // end parallel region

// RECURSION ROUTINE
static void rmss(const FunMapsVel *fmapsvel, const FunMapsPos *fmapspos, 
                 const Window *vtwin, double const * const sref_, const size_t nv, 
                 const size_t nt, const size_t ns, const double max_decel, 
                 const double total_dynmass, ArgOut *matout)
{
    const double Inf = (const double)mxGetInf();
    double prodiv = 0.;
    size_t k = ns - 1;
    // POSITIONAL LOOP
    while(k--){         // indices from ns - 2 to zero
        clock_t tloop = clock();
        int h;
        // get position dependent values
        const double sk         = sref_[k];                     // position s
        const double sstep      = sref_[k+1] - sk;              // current step width
        const double toff_sk    = vtwin->toffset_[k];           // time offset at current position
        const double toff_skp1  = vtwin->toffset_[k+1];         // time offset at next position
        const double voff_sk    = vtwin->voffset_[k];           // velocity offset at current position
        const double voff_skp1  = vtwin->voffset_[k+1];         // velocity offset at next position
        const double vmax_sk    = fmapspos->velolim_at_s_[k];   // velocity limit at current position
        const double vmax_skp1  = fmapspos->velolim_at_s_[k+1]; // velocity limit at next position
        // window time bound at next position
        const double tmax_skp1  = (vtwin->twidth-1/vtwin->tstep_r) + toff_skp1;
        // drag at current position
        const double route_drag_sk = fmapspos->route_drag_at_s_[k];   /
        // check for validity of matrix slice k+1 of costToEnd
        check_previous_mat(matout->costToEnd, k, nv, nt);
        // Check for user interruption
        if (utIsInterruptPending()) {        
            mexErrMsgIdAndTxt("MATLAB:dprm:userinterrupt",
                              "User interrupt detected.");
        }
#if PARALLEL
    #pragma omp parallel for private(h) num_threads(8) schedule(dynamic)
#endif
        for(h = 0; h < (int)nt; h++){
            size_t mk; 
            double tk;
            // current time
            tk = vtwin->trefloc_[h] + toff_sk;
            // INITIAL VELOCITY LOOP
            for(mk = 0; mk < nv-1; mk++){
                size_t mkp1 = nv-1;
                size_t tmp_iv, tmp_it;
                double tmp_f, tmp_e, tmp_t, tmp_c = Inf;
                // current velocity
                const double vk = vtwin->vrefloc_[mk] + voff_sk;
                // check for velocity limit exceedance
                if(vk > vmax_sk)
                    break;
                // TARGET VELOCITY LOOP
                while(mkp1--){ // 100 % executions
                    size_t t_skp1_idx, vmap_vq_idx, vmap_vk_idx, fmap_freq_idx;
                    double dv, vq, dt, aq, tkp1, F_DRAG, F_REQ, 
                            F_TRACTION_TRAIN, dE, cost, new_cost;
                    // next velocity
                    const double vkp1 = vtwin->vrefloc_[mkp1] + voff_skp1;
                    // check for velocity limit exceedance
                    if(vkp1 > vmax_skp1)
                        continue;
                    // --- DISCRETE SOLUTION OF EQUATION OF MOTION ---
                    // >>> mp * a = - Drag(sk, vk) + F_trac
                    // Difference equations
                    dv = vkp1 - vk;          // velocity stepwidth of current step
                    vq = (vkp1 + vk) * 0.5;  // average velocity on transition
                    dt = sstep / vq;         // time of transition
                    aq = dv / dt;            // average acceleration on transition
                    // Check celeration constraints
                    if(aq < -max_decel){
                        break; // 2.4 % of inner loop executions
                    }
                    tkp1 = dt + tk;
                    // Check for being within time window
                    if(tkp1 < toff_skp1){
                        continue;
                    }
                    if(tkp1 > tmax_skp1){
                        break;
                    }
                    // Compute time index
                    t_skp1_idx = (size_t)(((tkp1-toff_skp1) * vtwin->tstep_r) + 0.5);
                    // CHECK BOUNDS - t_skp1_idx
                    // (to prevent segfaults)
                    if(t_skp1_idx >= nt){
                        break;
                    }
                    // Check for valid follow up state
                    if(matout->costToEnd[IND3(mkp1, t_skp1_idx, k+1, nv, nt)] == Inf){
                        continue; // 24.4 % of inner loop executions
                    }
            // TRACTION FORCE COMPUTATION
                    // Get index for average velocity
                    vmap_vq_idx = (size_t)(vq*fmapsvel->vmapstep_r + 0.5);
                    // Compute total drag force: F(s, v)
                    F_DRAG = fmapsvel->train_drag_at_vq_[vmap_vq_idx] + route_drag_sk;
                    // ----- Equation of motion -----
                    // Compute required transition traction force
                    // F = m*p*a + Drag(s, v)
                    F_REQ = total_dynmass * aq + F_DRAG; 
                    // Differentiate + / - traction force
                    if(F_REQ > 0){
                        // stop iteration if maximum transmittable force is exceeded
                        if(F_REQ > fmapsvel->ftransmax){
                            continue; // 8.3 % of inner loop executions
                        }
                        // Get index for step divisible velocity
                        vmap_vk_idx = (size_t)(MAX(vkp1, vk) * fmapsvel->vmapstep_r + 0.5);
                        // Get max available traction force for max transition velocity
                        F_TRACTION_TRAIN = fmapsvel->trac_force_at_vq_[vmap_vk_idx];

                        if(F_REQ > F_TRACTION_TRAIN){
                            continue; // 10 % of inner loop executions
                        }
                    }
                    // Check for maximum negativ transmittable force, so that
                    // fmap_freq_idx will be within bounds
                    if(F_REQ < -fmapsvel->ftransmax){
                        break;
                    }
                    // --- all fine, acquire transition cost and save ---
                    // Get index for discretized traction force
                    fmap_freq_idx = (size_t)((F_REQ + fmapsvel->ftransmax) * fmapsvel->fmapstep_r + 0.5);
                    // CHECK BOUNDS for fmap_freq_idx
                    if(fmap_freq_idx >= fmapsvel->len_costfunc){
                        continue;
                    }
                    // 54.5 % of inner loop executions
                    cost = fmapsvel->costFunction[fmap_freq_idx + (vmap_vq_idx * fmapsvel->len_costfunc)];
                    dE = cost * dt; // our transition cost value (dE)
                    // Compute cost for current state
                    new_cost = dE + matout->costToEnd[IND3(mkp1, t_skp1_idx, k + 1, nv, nt)];
                    // Replace temporary transition is current transition is better
                    if(new_cost < tmp_c){
                        tmp_c = new_cost;
                        tmp_it = t_skp1_idx;
                        tmp_iv = mkp1;
                        tmp_f = F_REQ;
                        tmp_e = dE;
                        tmp_t = dt;
                    }
                } // END TARGET VELOCITY LOOP

                // Store transition to output
                if(tmp_c < Inf){
                    size_t idx = IND3(mk, h, k, nv, nt);
                    matout->costToEnd[idx]  = tmp_c;
                    matout->optV_ind[idx]  = (uint8_T )tmp_iv + 1; // MATLAB index conversion
                    matout->optT_ind[idx]  = (uint16_T)tmp_it + 1; // by increment
                    matout->transTimes[idx] = tmp_t;

                    if(matout->save_all){
                        matout->transForces[idx]   = tmp_f;
                        matout->transEnergies[idx] = tmp_e;
                    }
                    tmp_c = Inf;
                }
            } // END INIT VELOCITY LOOP
        } // END TIME LOOP
        // print loop state
        print_state(tloop, k+1, ns, &prodiv);
    } // END POSITIONAL LOOP
    // END OF RECURSION
}