Merge branch 'master' of github.com:flforget/DDP

cpp/include/ilqrsolver.h

@@ -78,7 +78,7 @@ public:
     void initTrajectory();
     void backwardLoop();
     void forwardLoop();
-    char isQuudefinitePositive(commandMat_t& Quu);
+    bool isQuudefinitePositive(const commandMat_t & Quu);
     struct traj getLastSolvedTrajectory();
 private:
 protected:

cpp/src/ilqrsolver.cpp

@@ -162,10 +162,21 @@ ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
     return lastTraj;
 }
 
-char ILQRSolver::isQuudefinitePositive(commandMat_t& Quu)
+bool ILQRSolver::isQuudefinitePositive(const commandMat_t & Quu)
 {
     /*
       Todo : check if Quu is definite positive
     */
-    return 1;
+    //Eigen::JacobiSVD<commandMat_t> svd_Quu (Quu, ComputeThinU | ComputeThinV);
+    Eigen::VectorXcd singular_values = Quu.eigenvalues();
+
+    for(long i = 0; i < Quu.cols(); ++i)
+    {
+        if (singular_values[i].real() < 0.)
+        {
+            std::cout << "not sdp" << std::endl;
+            return false;
+        }
+    }
+    return true;
 }