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<=N;i++)

compute matrix G(G=G¡¤Ai-1, G if i=1)

store 4th column of G as Pi-1 (3 element vector)

store 3th column of G as Zi-1

compute column i of Jacobian using contiguous

( [dx i , dy i , dz]T=Zi-1¡Á(PN-Pi-1) [¦Äx,¦Äy,¦Äz]T=Zi-1 )

}

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<=N;i++)

compute matrix G(G=G¡¤Ai-1, G if i=1)

store 4th column of G as Pi-1 (3 element vector)

store 3th column of G as Zi-1

compute column i of Jacobian using contiguous

( [dx i , dy i , dz]T=Zi-1¡Á(PN-Pi-1) [¦Äx,¦Äy,¦Äz]T=Zi-1 )

}