Help, it's my understading of the Inverse kinematics algorithm right?

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 d0;
While(||Jd-dx||ܦšIterNumMaxIterNum)
{
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=dXJ-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=GAi-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 )
}
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
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 d0;
While(||Jd-dx||ܦšIterNumMaxIterNum)
{
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=dXJ-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=GAi-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 )
}
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.