People have recommended me implement an analytic version of inverse Jacobian solver, such that I won't be forced only the least square solution, but would have an local area of solution near to the one I desire.
I can't seem to implement it correctly, I mean how much does it differ from the least square inverse kinematics which I have implemented here?
Eigen::MatrixXd jq(device_.get()->baseJend(state).e().cols(),device_.get()->baseJend(state).e().rows()); jq = device_.get()->baseJend(state).e(); //Extract J(q) directly from robot //Least square solver - [AtA]⁻1AtB Eigen::MatrixXd A (6,6); A = jq.transpose()*(jq*jq.transpose()).inverse(); Eigen::VectorXd du(6); du(0) = 0.1 - t_tool_base.P().e(); du(1) = 0 - t_tool_base.P().e(); du(2) = 0 - t_tool_base.P().e(); du(3) = 0; // Should these be set to something if i don't want the tool position to rotate? du(4) = 0; du(5) = 0; ROS_ERROR("What you want!"); Eigen::VectorXd q(6); q = A*du; cout << q << endl; // Least square solution - want a vector of solutions.
I want a vector of solution - how do I get that?
The robot being used is a UR5 - https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf