I want implement the IK algorithm,but I haven't the slightest idea about IK before, after reading some papers and lecuture I downloaded from the internet, I still cann't fully understand it, the following Pseudocode embodies my idea, I really need your help to make me master the algorithm. By the way, I just want to use IK to drive the articulated body.
Pseudocode:
input initial estimate of d¦È0;
While(||J¡¤d¦È-dx||¡??ÅIterNum¡ÝMaxIterNum)
{
EstablishJacobian(d¦È);//establish the Jacobian matrix;
calculate dX;
calculate calculate J-1;(using Gauss eliminating method if the matrix is square, else use pseudo inversion)
calculate d¦È using equation d¦È=dX¡¤J-1;
}
EstablishJacobian(d¦È)
{
compute end effector transformation T 6
store 4th column of T 6 as P N
for( i=1;iI