Commit b0b07ef9 authored by flforget's avatar flforget
Browse files

check if Quu if definite positive

parent 1e6d36c8
......@@ -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:
......
......@@ -161,10 +161,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;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment