int main(int argc, char** argv) {
ros::init(argc, argv, "eigen_test");
ros::NodeHandle nh_jntPub; // node handle for joint command publisher
// ros::Publisher pub_joint_commands; //
// pub_joint_commands = nh_jntPub.advertise<atlas_msgs::AtlasCommand>("/atlas/atlas_command", 1, true);
ROS_INFO("test eigen program");
Eigen::Matrix3f A;
Eigen::Vector3f b;
A << 1,2,3, 4,5,6, 7,8,10;
A(1,2)=0; // how to access one element of matrix; start from 0; no warning out of range...
b << 3,3,4;
std::cout <<"b = "<<b <<std::endl;
// column operaton: replace first column of A with vector b:
A.col(0)= b; // could copy columns of matrices w/ A.col(0) = B.col(0);
std::cout <<"A = "<<A <<std::endl;
Eigen::MatrixXd mat1 = Eigen::MatrixXd::Zero(6, 6); //6x6 matrix full of zeros
Eigen::MatrixXd mat2 = Eigen::MatrixXd::Identity(6, 6); //6x6 identity matrix
std::cout<<mat1<<std::endl;
std::cout<<mat2<<std::endl;
Eigen::Vector3f xtest = A.colPivHouseholderQr().solve(b);
std::cout<<"soln xtest = "<<xtest<<std::endl;
Eigen::Vector3f x = A.partialPivLu().solve(b); //dec.solve(b); //A.colPivHouseholderQr().solve(b);
std::cout<<"soln x = "<<x<<std::endl;
Eigen::Vector3f btest = A*x;
std::cout<<"test soln: A*x = " <<btest<<std::endl;
//extend to 6x6 test: v = M*z, find z using 2 methods
// use double-precision matrices/vectors
Eigen::MatrixXd M = Eigen::MatrixXd::Random(6,6);
std::cout<<"test 6x6: M = "<<M<<std::endl;
Eigen::VectorXd v(6);
v << 1,2,3,4,5,6;
std::cout<<"v = "<<v<<std::endl;
Eigen::VectorXd z(6);
Eigen::VectorXd ztest(6);
ztest = M.colPivHouseholderQr().solve(v);
std::cout<<"soln ztest = "<<ztest<<std::endl;
z = M.partialPivLu().solve(v);
std::cout<<"soln 6x6: z = "<<z<<std::endl;
Eigen::VectorXd vtest(6);
vtest = M*z;
std::cout<<"test soln: M*z = "<<vtest<<std::endl;
// .norm() operator...
double relative_error = (M*z - v).norm() / v.norm(); // norm() is L2 norm
std::cout << "The relative error is:\n" << relative_error << std::endl;
std::cout<<"dot prod, v, z: "<< v.dot(z)<<std::endl;
std::cout<<"cross prod, b-cross-x: " << b.cross(x)<<std::endl;
return 0;
}
int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
Eigen::VectorXd* _z) {
int n = _q.size();
const double zer_tol = 1e-5;
const double piv_tol = 1e-8;
int maxiter = 1000;
int err = 0;
if (_q.minCoeff() > 0) {
// LOG(INFO) << "Trivial solution exists.";
*_z = Eigen::VectorXd::Zero(n);
return err;
}
*_z = Eigen::VectorXd::Zero(2 * n);
int iter = 0;
double theta = 0;
double ratio = 0;
int leaving = 0;
Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
Eigen::VectorXd x = _q;
std::vector<int> bas;
std::vector<int> nonbas;
int t = 2 * n + 1;
int entering = t;
bas.clear();
nonbas.clear();
for (int i = 0; i < n; ++i) {
bas.push_back(i);
}
Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);
for (int i = 0; i < bas.size(); ++i) {
B.col(bas[i]) = _M.col(bas[i]);
}
x = -B.partialPivLu().solve(_q);
Eigen::VectorXd minuxX = -x;
int lvindex;
double tval = minuxX.maxCoeff(&lvindex);
leaving = bas[lvindex];
bas[lvindex] = t;
Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
for (int i = 0; i < n; ++i) {
if (x[i] < 0)
U[i] = 1;
}
Be = -(B * U);
x += tval * U;
x[lvindex] = tval;
B.col(lvindex) = Be;
for (iter = 0; iter < maxiter; ++iter) {
if (leaving == t) {
break;
} else if (leaving < n) {
entering = n + leaving;
Be = Eigen::VectorXd::Zero(n);
Be[leaving] = -1;
} else {
entering = leaving - n;
Be = _M.col(entering);
}
Eigen::VectorXd d = B.partialPivLu().solve(Be);
std::vector<int> j;
for (int i = 0; i < n; ++i) {
if (d[i] > piv_tol)
j.push_back(i);
}
if (j.empty()) {
// err = 2;
break;
}
int jSize = static_cast<int>(j.size());
Eigen::VectorXd minRatio(jSize);
for (int i = 0; i < jSize; ++i) {
minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
}
double theta = minRatio.minCoeff();
std::vector<int> tmpJ;
std::vector<double> tmpMinRatio;
for (int i = 0; i < jSize; ++i) {
if (x[j[i]] / d[j[i]] <= theta) {
tmpJ.push_back(j[i]);
tmpMinRatio.push_back(minRatio[i]);
}
}
// if (tmpJ.empty())
// {
// LOG(WARNING) << "tmpJ should never be empty!!!";
// LOG(WARNING) << "dumping data:";
// LOG(WARNING) << "theta:" << theta;
// for (int i = 0; i < jSize; ++i)
// {
// LOG(WARNING) << "x(" << j[i] << "): " << x[j[i]] << "d: " << d[j[i]];
// }
// }
j = tmpJ;
jSize = static_cast<int>(j.size());
if (jSize == 0) {
err = 4;
break;
}
lvindex = -1;
for (int i = 0; i < jSize; ++i) {
if (bas[j[i]] == t)
lvindex = i;
}
if (lvindex != -1) {
lvindex = j[lvindex];
} else {
theta = tmpMinRatio[0];
lvindex = 0;
for (int i = 0; i < jSize; ++i) {
if (tmpMinRatio[i]-theta > piv_tol) {
theta = tmpMinRatio[i];
lvindex = i;
}
}
lvindex = j[lvindex];
}
leaving = bas[lvindex];
ratio = x[lvindex] / d[lvindex];
bool bDiverged = false;
for (int i = 0; i < n; ++i) {
if (isnan(x[i]) || isinf(x[i])) {
bDiverged = true;
break;
}
}
if (bDiverged) {
err = 4;
break;
}
x = x - ratio * d;
x[lvindex] = ratio;
B.col(lvindex) = Be;
bas[lvindex] = entering;
}
if (iter >= maxiter && leaving != t)
err = 1;
if (err == 0) {
for (size_t i = 0; i < bas.size(); ++i) {
if (bas[i] < _z->size()) {
(*_z)[bas[i]] = x[i];
}
}
Eigen::VectorXd realZ = _z->segment(0, n);
*_z = realZ;
if (!validate(_M, *_z, _q)) {
// _z = VectorXd::Zero(n);
err = 3;
}
} else {
*_z = Eigen::VectorXd::Zero(n); // solve failed, return a 0 vector
}
// if (err == 1)
// LOG(ERROR) << "LCP Solver: Iterations exceeded limit";
// else if (err == 2)
// LOG(ERROR) << "LCP Solver: Unbounded ray";
// else if (err == 3)
// LOG(ERROR) << "LCP Solver: Solver converged with numerical issues. "
// << "Validation failed.";
// else if (err == 4)
// LOG(ERROR) << "LCP Solver: Iteration diverged.";
return err;
}
