Merge pull request #50 from alexliniger/fullsize_fast

fix in angle unwrapping and more faster tuning
alexliniger · Sep 4, 2020 · 302e12a · 302e12a
2 parents 1a36827 + fd869cc
commit 302e12a
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 19 deletions.
diff --git a/C++/Cost/cost.cpp b/C++/Cost/cost.cpp
@@ -49,10 +49,10 @@ TrackPoint Cost::getRefPoint(const ArcLengthSpline &track,const State &x) const
     // curvature
     double dtheta_ref_nom = (dx_ref*ddy_ref - dy_ref*ddx_ref);
     double dtheta_ref_denom = (dx_ref*dx_ref + dy_ref*dy_ref);
-    if(std::fabs(dtheta_ref_nom) < 1e-7)
-        dtheta_ref_nom = 0;
-    if(std::fabs(dtheta_ref_denom) < 1e-7)
-        dtheta_ref_denom = 1e-7;
+    // if(std::fabs(dtheta_ref_nom) < 1e-7)
+    //     dtheta_ref_nom = 0;
+    // if(std::fabs(dtheta_ref_denom) < 1e-7)
+    //     dtheta_ref_denom = 1e-7;
     double dtheta_ref = dtheta_ref_nom/dtheta_ref_denom;
 
     return {x_ref,y_ref,dx_ref,dy_ref,theta_ref,dtheta_ref};

diff --git a/C++/MPC/mpc.cpp b/C++/MPC/mpc.cpp
@@ -158,7 +158,7 @@ void MPC::unwrapInitialGuess()
     double L = track_.getLength();
     for(int i=1;i<=N;i++)
     {
-        if((initial_guess_[i].xk.phi - initial_guess_[i-1].xk.phi) < M_PI)
+        if((initial_guess_[i].xk.phi - initial_guess_[i-1].xk.phi) < -M_PI)
         {
             initial_guess_[i].xk.phi += 2.*M_PI;
         }

diff --git a/C++/Params/cost.json b/C++/Params/cost.json
@@ -1,26 +1,26 @@
 
 {
-  "qC" : 5.0,
+  "qC" : 0.01,
   "qL" : 1000.0,
-  "qVs" : 0.1,
+  "qVs" : 0.2,
 
-  "qMu": 1.0,
+  "qMu": 0.1,
 
   "qR" : 1E-4,
 
-  "qBeta" : 5E1,
+  "qBeta" : 5E0,
   "betaKin": 1,
 
   "rD" : 1E-3,
   "rDelta" : 1E-3,
   "rVs" : 1E-5,
 
-  "rdD" : 1E0,
-  "rdDelta" : 1E2,
-  "rdVs" : 1E-3,
+  "rdD" : 1E-1,
+  "rdDelta" : 5E2,
+  "rdVs" : 1E-4,
 
   "qCNmult": 1000.0,
-  "qRNmult": 1000.0,
+  "qRNmult": 10.0,
 
   "sc_quad_track": 100.0,
   "sc_quad_tire": 100.0,

diff --git a/C++/Params/model.json b/C++/Params/model.json
@@ -24,8 +24,8 @@
 
   "g"   : 9.81,
 
-  "R_in" : 1.0,
-  "R_out" : 1.0,
+  "R_in" : 2.0,
+  "R_out" : 2.0,
 
   "max_dist_proj": 3.0,
 
@@ -34,7 +34,7 @@
 
   "maxAlpha" : 0.15,
 
-  "initial_velocity" : 20.0,
+  "initial_velocity" : 15.0,
   "s_trust_region" : 30.0,
 
   "vx_zero": 0.3

diff --git a/C++/Params/track.cpp b/C++/Params/track.cpp
@@ -43,8 +43,8 @@ Track::Track(std::string file)
 
 TrackPos Track::getTrack()
 {
-    return {1*Eigen::Map<Eigen::VectorXd>(X.data(), X.size()), 1*Eigen::Map<Eigen::VectorXd>(Y.data(), Y.size()),
-            1*Eigen::Map<Eigen::VectorXd>(X_inner.data(), X_inner.size()), 1*Eigen::Map<Eigen::VectorXd>(Y_inner.data(), Y_inner.size()),
-            1*Eigen::Map<Eigen::VectorXd>(X_outer.data(), X_outer.size()), 1*Eigen::Map<Eigen::VectorXd>(Y_outer.data(), Y_outer.size())};
+    return {1.0*Eigen::Map<Eigen::VectorXd>(X.data(), X.size()), 1.0*Eigen::Map<Eigen::VectorXd>(Y.data(), Y.size()),
+            1.0*Eigen::Map<Eigen::VectorXd>(X_inner.data(), X_inner.size()), 1.0*Eigen::Map<Eigen::VectorXd>(Y_inner.data(), Y_inner.size()),
+            1.0*Eigen::Map<Eigen::VectorXd>(X_outer.data(), X_outer.size()), 1.0*Eigen::Map<Eigen::VectorXd>(Y_outer.data(), Y_outer.size())};
 }
 }