diff --git a/common/config/reduced_mu_turn_nonadapt_config.yaml b/common/config/reduced_mu_turn_nonadapt_config.yaml
index 80e288f..6961793 100644
--- a/common/config/reduced_mu_turn_nonadapt_config.yaml
+++ b/common/config/reduced_mu_turn_nonadapt_config.yaml
@@ -4,8 +4,8 @@ t_activate: 15.0
 t_final: 500.0
 scenario_id: 2 # 1: pop-up, 2: reduced mu turn, 3: racing
 s_begin_log: 180
-s_pause_gazebo: []
-#s_pause_gazebo: [180, 195, 210, 225, 240] # snapshots
+#s_pause_gazebo: []
+s_pause_gazebo: [180, 195, 210, 225, 240] # snapshots
 
 # pop-up obstacle config
 s_ego_at_popup:  50
diff --git a/saarti/src/cuda_wrapper.cu b/saarti/src/cuda_wrapper.cu
index 8ce7169..1802c48 100644
--- a/saarti/src/cuda_wrapper.cu
+++ b/saarti/src/cuda_wrapper.cu
@@ -11,22 +11,31 @@ __global__ void single_rollout(float *trajset_arr,
                                float *x_init,
                                uint Npath,
                                float *s_path,
+                               float *vxref_path,
                                float *kappac_path,
+                               float *mu_path,
+                               float mu_nominal,
                                float m,
                                float Iz,
                                float lf,
                                float lr,
+                               float h_cg,
+                               float g,
                                uint Nt,
                                uint Ni,
                                uint Nx,
                                uint Nu,
                                uint Ntrajs,
                                uint Npp,
-                               float dt)
+                               float dt,
+                               int traction_adaptive,
+                               int ref_mode,
+                               float vxref_nominal,
+                               float *d_ref)
 {
     // notes on indexing:
     // trajset_arr:
-    // row (i) <- state (Nx+Nu)
+    // row (i) <- state (Nx+Nu+Nmisc)
     // column (j) <- traj (Ntrajs)
     // page (k) <- time (Nt)
     // iii - index of the 1d array that represents the 3d array (increase order: rows - columns - pages)
@@ -49,23 +58,147 @@ __global__ void single_rollout(float *trajset_arr,
     trajset_arr[threadIdx.x + 5*Ntrajs + 0*Npp] = vy;
 
     // rollout loop
-    for (int ki=0; ki<(Nt*Ni)-1; ki++){
+    float Fyf = 0.0f;
+    float Fxf = 0.0f;
+    float Fxr = 0.0f;
+    uint k = 0; // k is the regular iterator, ki is upsampled
+    for (int ki=0; ki<((Nt+1)*Ni); ki++){
 
-        // get kappac at s (todo!)
-        for (int id= 0; id<Npath; ++id) {
-            // break if spath - s is positive, save index
+        // get kappac at s
+        int path_idx;
+        for (path_idx = 0; path_idx<Npath; path_idx++) {
+            // break if s - spath is negative, save index
+            if(s-s_path[path_idx] <= 0){
+                break;
+            }
         }
-        float kappac = 0;
+        float kappac = kappac_path[path_idx];
 
-        // set control input
-        float Fyf = 10000*((float(threadIdx.x)/float(Ntrajs))-0.5f);
-        float Fxf = 500;
-        float Fxr = 500;
+        // set ax for Fz computation from previous cmd
+        float ax = (Fxf+Fxr)/m;
+
+        // get normal forces front and back and mu
+        float Fzf;
+        float Fzr;
+        float mu;
+        if(traction_adaptive == 1){
+            float theta = 0; // grade angle todo get from pathlocal via traj
+            Fzf = (1.0f/(lf+lr))*(m*ax*h_cg  - m*g*h_cg*sin(theta) + m*g*lr*cos(theta));
+            Fzr = (1.0f/(lf+lr))*(-m*ax*h_cg + m*g*h_cg*sin(theta) + m*g*lf*cos(theta));
+            mu = mu_path[path_idx]; // get mu from pathlocal at s
+        } else { // (traction_adaptive == 0)
+            Fzf = (1.0f/(lf+lr))*(m*g*lr);
+            Fzr = (1.0f/(lf+lr))*(m*g*lf);
+            mu = mu_nominal;
+        }
+
+        // get rear cornering stiffness
+        float B, C, D; //, E;
+        // todo add case for racing
+        if(0.0f <= mu && mu <0.3f){ // ice
+            B = 4.0f;
+            C = 2.0f;
+            D = mu;
+            //E = 1.0f;
+        } else if (0.3f <= mu && mu <0.5f) { // snow
+            B = 5.0f;
+            C = 2.0f;
+            D = mu;
+            //E = 1.0f;
+        } else if (0.5f <= mu && mu <0.9f) { // wet
+            B = 12.0f;
+            C = 2.3f;
+            D = mu;
+            //E = 1.0f;
+        } else if (0.9f <= mu && mu <2.5f) { // dry
+            B = 10.0f;
+            C = 1.9f;
+            D = mu;
+            //E = 0.97f;
+        } else {
+            // todo Error = nonzero nr - check at host and throw error
+        }
+        float Cr = B*C*D*Fzr; // Rajamani
 
         // set Fyr
-        float Cr = 200000; // todo get dynamically!
         float Fyr = 2*Cr*atan(lr*psidot-vy)/vx; // help variable
 
+        // compute maximum tire forces
+        float Ffmax = mu*Fzf;
+        float Frmax = mu*Fzr;
+
+        // get local vxref and vxerror
+        float vxref;
+        if(ref_mode == 1){ // cc
+            vxref = vxref_nominal;
+        } else if(ref_mode == 2) { // max s
+            vxref = vxref_path[path_idx];
+        } else {
+            vxref = 0; // minimize s (refmode 0)
+        }
+        float vxerror = vxref-vx; // todo use blockIdx.x here
+
+        // get derror (one per thread)
+        float derror = d_ref[threadIdx.x] - d;
+
+        /*
+         * ROLLOUT CONTROLLER
+         */
+
+        // TODO CHECK THAT WEE GET SIMILAR OUTPUT FROM EULER AND RK4
+        // TODO GET PARAMS FOR STARNDARD STATE FEEDBACK
+
+        // select Fyf
+        float feedfwd = 0.5f*m*vx*vx*kappac*cos(deltapsi);
+        float feedback = 3000*derror - 500*deltapsi;
+        Fyf = feedfwd + feedback;
+
+        // saturate Fyf at Ffmax
+        if(Fyf >= Ffmax){
+            Fyf = Ffmax;
+        }
+        if(Fyf<=-Ffmax){
+            Fyf = -Ffmax;
+        }
+
+        // select Fxf
+        float Fxfmax = sqrt(Ffmax*Ffmax-Fyf*Fyf);
+        if (ref_mode == 0){ // minimize s
+            // select max negative Fxf until stop
+            Fxf = -Fxfmax;
+        } else if (ref_mode == 1 || ref_mode == 2){ // maximize s or cc
+            if(vxerror > 0){ // accelerating
+                Fxf = 0; // rear wheel drive - no drive on front wheel
+            } else { // braking
+                Fxf = 1000*vxerror;
+                // saturate
+                if(Fxf<=-Fxfmax){
+                    Fxf = -Fxfmax;
+                }
+            }
+        }
+
+        // select Fxr
+        //float Fxrmax = mu*Fzr;
+        float Fxrmax = sqrt(Frmax*Frmax-Fyr*Fyr);
+        if (ref_mode == 0){ // minimize s
+            Fxr = -Fxrmax;
+        } else if (ref_mode == 1 || ref_mode == 2){ // maximize s or cc
+            Fxr = 1000*vxerror;
+            // saturate
+            if(Fxr >= Fxrmax){
+                Fxr = Fxrmax;
+            }
+            if(Fxr<=-Fxrmax){
+                Fxr = -Fxrmax;
+            }
+        }
+
+        // old
+        // Fyf = 10000*((float(threadIdx.x)/float(Ntrajs))-0.5f);
+        // Fxf = 500;
+        // Fxr = 500;
+
         // euler fwd step
         s        = s + (dt/Ni)*((vx*cos(deltapsi)-vy*sin(deltapsi))/(1-d*kappac));
         d        = d + (dt/Ni)*(vx*sin(deltapsi)+vy*cos(deltapsi));
@@ -75,7 +208,7 @@ __global__ void single_rollout(float *trajset_arr,
         vy       = vy + (dt/Ni)*((1/m)*(Fyf+Fyr)-vx*psidot);
 
         if(ki % Ni == 0){
-            uint k = ki/Ni;
+            k = ki/Ni;
             // set x at k+1
             trajset_arr[threadIdx.x + 0*Ntrajs + (k+1)*Npp] = s;
             trajset_arr[threadIdx.x + 1*Ntrajs + (k+1)*Npp] = d;
@@ -87,6 +220,12 @@ __global__ void single_rollout(float *trajset_arr,
             trajset_arr[threadIdx.x + 6*Ntrajs + (k)*Npp] = Fyf;
             trajset_arr[threadIdx.x + 7*Ntrajs + (k)*Npp] = Fxf;
             trajset_arr[threadIdx.x + 8*Ntrajs + (k)*Npp] = Fxr;
+            // set miscvars
+            trajset_arr[threadIdx.x + 9*Ntrajs + (k)*Npp] = Fzf;
+            trajset_arr[threadIdx.x + 10*Ntrajs + (k)*Npp] = Fzr;
+            trajset_arr[threadIdx.x + 11*Ntrajs + (k)*Npp] = kappac;
+            trajset_arr[threadIdx.x + 12*Ntrajs + (k)*Npp] = mu;
+            trajset_arr[threadIdx.x + 13*Ntrajs + (k)*Npp] = Cr;
         }
     }
 }
@@ -98,6 +237,8 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
                   containers::staticparamstruct sp,
                   int traction_adaptive,
                   float mu_nominal,
+                  containers::refstruct refs,
+                  float vxref_nominal,
                   uint Nt, // N in planning horizon
                   uint Ni, // scaling factor in integration
                   uint Ntrajs, // Nr of trajs to roll out TODO replace w. Nd and Nvx
@@ -107,23 +248,26 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
     // init variables
     uint Nx = 6;
     uint Nu = 3;
-    uint Npp = (Nx+Nu)*Ntrajs; // elements_per_page
+    uint Nmisc = 5; // nr of additional traj vars
+    uint Npp = (Nx+Nu+Nmisc)*Ntrajs; // elements_per_page
     float *trajset_arr;
     float *x_init;
     float *d_ref;
 
     uint Npath = pathlocal.s.size();
     float *s_path;
+    float *vxref_path;
     float *kappac_path;
-    //float *mu_path;
+    float *mu_path;
 
     // allocate shared memory
-    cudaMallocManaged(&trajset_arr, (Nx+Nu)*Ntrajs*Nt*sizeof(float));
+    cudaMallocManaged(&trajset_arr, (Nx+Nu+Nmisc)*Ntrajs*Nt*sizeof(float));
     cudaMallocManaged(&x_init, Nx*sizeof(float));
     cudaMallocManaged(&d_ref, Ntrajs*sizeof(float));
     cudaMallocManaged(&s_path, Npath*sizeof(float));
+    cudaMallocManaged(&vxref_path, Npath*sizeof(float));
     cudaMallocManaged(&kappac_path, Npath*sizeof(float));
-    //cudaMallocManaged(&mu_path, Npath*sizeof(float));
+    cudaMallocManaged(&mu_path, Npath*sizeof(float));
 
     // set init state
     x_init[0] = initstate.s;
@@ -137,12 +281,12 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
     float dlb = pathlocal.dlb.at(0);
     float dub = pathlocal.dub.at(0);
     float dstep = (dub-dlb)/Ntrajs;
-    for(int id=0; id<Ntrajs; ++id) {
+    for(uint id=0; id<Ntrajs; ++id) {
         d_ref[id] = dlb+id*dstep;
     }
 
     // set path variables
-    for(int id=0; id<Npath; ++id) {
+    for(uint id=0; id<Npath; ++id) {
         s_path[id] = pathlocal.s.at(id);
         kappac_path[id] = pathlocal.kappa_c.at(id);
         //mu_path[id] = pathlocal.mu.at(id);
@@ -153,18 +297,27 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
                                   x_init,
                                   Npath,
                                   s_path,
+                                  vxref_path,
                                   kappac_path,
+                                  mu_path,
+                                  mu_nominal,
                                   sp.m,
                                   sp.Iz,
                                   sp.lf,
                                   sp.lr,
+                                  sp.h_cg,
+                                  sp.g,
                                   Nt,
                                   Ni,
                                   Nx,
                                   Nu,
                                   Ntrajs,
                                   Npp,
-                                  dt);
+                                  dt,
+                                  traction_adaptive,
+                                  refs.ref_mode,
+                                  vxref_nominal,
+                                  d_ref);
 
     // wait for GPU to finish before accessing on host
     cudaDeviceSynchronize();
@@ -180,10 +333,27 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
             traj.psidot.push_back(trajset_arr[j + 3*Ntrajs + k*Npp]);
             traj.vx.push_back(trajset_arr[j + 4*Ntrajs + k*Npp]);
             traj.vy.push_back(trajset_arr[j + 5*Ntrajs + k*Npp]);
+            if(k<Nt){ // N+1 states and N ctrls
+                traj.Fyf.push_back(trajset_arr[j + 6*Ntrajs + k*Npp]);
+                traj.Fxf.push_back(trajset_arr[j + 7*Ntrajs + k*Npp]);
+                traj.Fxr.push_back(trajset_arr[j + 8*Ntrajs + k*Npp]);
+                // miscvars
+                traj.Fzf.push_back(trajset_arr[j + 9*Ntrajs + k*Npp]);
+                traj.Fzr.push_back(trajset_arr[j + 10*Ntrajs + k*Npp]);
+                traj.kappac.push_back(trajset_arr[j + 11*Ntrajs + k*Npp]);
+                traj.mu.push_back(trajset_arr[j + 12*Ntrajs + k*Npp]);
+                traj.Cr.push_back(trajset_arr[j + 13*Ntrajs + k*Npp]);
+            }
         }
+        // add last element of misc vars
+        traj.Fzf.push_back(traj.Fzf.back());
+        traj.Fzr.push_back(traj.Fzr.back());
+        traj.kappac.push_back(traj.kappac.back());
+        traj.mu.push_back(traj.mu.back());
+        traj.Cr.push_back(traj.Cr.back());
         trajset_struct.push_back(traj);
     }
-    std::cout << "reached end of cuda_rollout" << std::endl;
+    //std::cout << "reached end of cuda_rollout" << std::endl;
 
     // Free memory
     cudaFree(trajset_arr);
diff --git a/saarti/src/rtisqp_wrapper.cpp b/saarti/src/rtisqp_wrapper.cpp
index 3544a46..7559b1b 100644
--- a/saarti/src/rtisqp_wrapper.cpp
+++ b/saarti/src/rtisqp_wrapper.cpp
@@ -567,7 +567,7 @@ void RtisqpWrapper::computeTrajset(vector<containers::trajstruct> &trajset,
             if(k==0){
                 traj.ax.push_back(0);
             } else {
-                traj.ax.push_back( (traj.Fxf.at(k-1)+traj.Fyf.at(k-1))/sp.m );
+                traj.ax.push_back( (traj.Fxf.at(k-1)+traj.Fxr.at(k-1))/sp.m );
             }
 
             // compute normal forces front and back
diff --git a/saarti/src/saarti_node.cpp b/saarti/src/saarti_node.cpp
index dbf5f11..208233f 100644
--- a/saarti/src/saarti_node.cpp
+++ b/saarti/src/saarti_node.cpp
@@ -7,6 +7,8 @@ void cuda_rollout(std::vector<containers::trajstruct> &trajset_struct,
                   containers::staticparamstruct sp,
                   int traction_adaptive,
                   float mu_nominal,
+                  containers::refstruct refs,
+                  float vxref_nominal,
                   uint Nt,
                   uint Ni,
                   uint Ntrajs,
@@ -123,11 +125,22 @@ SAARTI::SAARTI(ros::NodeHandle nh){
             if(sampling_augmentation_ == 1){
                 ROS_INFO_STREAM("generating trajectory set");
                 // cpu rollout
-                rtisqp_wrapper_.computeTrajset(trajset_,state_,pathlocal_,sp_,traction_adaptive_,mu_nominal_,vxref_cc_,refs_,uint(Ntrajs_rollout_));
+                //rtisqp_wrapper_.computeTrajset(trajset_,state_,pathlocal_,sp_,traction_adaptive_,mu_nominal_,vxref_cc_,refs_,uint(Ntrajs_rollout_));
 
                 // gpu rollout
                 uint Ni = 10;
-                cuda_rollout(trajset_, state_, pathlocal_, sp_, traction_adaptive_, mu_nominal_, N, Ni, uint(Ntrajs_rollout_),dt_);
+                cuda_rollout(trajset_,
+                             state_,
+                             pathlocal_,
+                             sp_,
+                             traction_adaptive_,
+                             mu_nominal_,
+                             refs_,
+                             vxref_cc_,
+                             N,
+                             Ni,
+                             uint(Ntrajs_rollout_),
+                             dt_);
 
                 // append trajprime
                 if (trajstar_last.s.size()!=0){