Use of Jacobian

[Boaxy]

Hey,
I hope someone here will be able to help me. I have recently been developing code for a 3D robotic arm using the Java3D API. I had asked for help on some Java3D forums, and they said I'd be better asking this in a robotics forum instead.

I have created a program that allows the user to choose which type of robot to use (Cartesian, 2-link planar cylindrical, non planar Cylindrical, Articulated and SCARA), and the robot is made in a 3D java virtual universe using cylinders for links and spheres for joints. Right now I have the forward kinematics working on all of the robots, so I have equations for all of them. The stage I'm currently at is trying to allow the user to input x, y and z values and have the robot simulate the movement to this point. I have the cartesian robot working in this way, as that was simply a case of rearranging the equation. I am now working on the 2-link planar.
To do this, I am trying to make use of the Jacobian and it's inverse.

The code I have written to do this is as follows:
//dt1dx, dt2dx, dt1dy, dt2dy are the partial derivatives that fill the jacobian
Matrix4d Jacobian = new Matrix4d(dt1dx, dt2dx, 0, 0, dt1dy, dt2dy, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
jInverse = pseudoInverse(Jacobian); //Finds the inverse
double dTheta1 = jInverse.m00*dX.x + jInverse.m01*dX.y; //Multiplies jInverse by the
double dTheta2 = jInverse.m10*dX.x + jInverse.m11*dX.y; //vector dX
theta1 += dTheta1; //Adds dTheta1 to theta1
theta2 += dTheta2; //Adds dTheta2 to theta2

dX is calculated by interpolating between the End-Effector and the target at increments of 0.005, and this is interpolated from 0 to 1.

The theta1 and theta2 values are then passed into methods which rotate the particular joint they are associated with.

I know this isn't the sort of stuff you guys usually deal with on here, as there isn't a physical robot involved, but since it is to do with the maths behind how the robot actually works, I think you guys are the most equipped to help me.
Any help would be greatly appreciated.
[EDIT] forgot to say what my actual problem was, lol.
The problem is that the robot seems to move smothly, but doesn't move to the point specified. I just seems to swing around for a bit before coming to a stop. When I multiplied the jacobian by jInverse, the outcome was always very close to the Identity matrix, so I'm assuming my pseudoInverse function is OK.
I'm pretty sure my partial derivatives are OK too, as I have checked these again and again, and always get the same PD's out.
Thanks again for any help.

[parallax]

I am really interested in this as well, but I'm struggling with the math. I found this video awhile back that uses Damped Least squares method with the Jacobian to calculate the IK for a 5 degree of freedom arm. It may help you or it may not. Here it is:

[youtube]lBLyaYvRvYc[/youtube]

I will be following this thread closely   

[Boaxy]

Thanks for the reply,
The video was quite helpful with some slight oscillating problems I was having, but I already sort of understand everything else in the video. But I got your personal message and might have a look for him at the other forum.
I think It's close to working, and when I finally work it out it is going to be something stupid that was stopping it working (Thats always the way..). It is moving smoothly, just not to where it should be, lol.
If you'd like when I finish the project, will have to be done by the end of next month, I can post the .jar file or send you the source code.

[parallax]

Glad I was able to help out a little. 

I think It's close to working, and when I finally work it out it is going to be something stupid that was stopping it working (Thats always the way..). It is moving smoothly, just not to where it should be, lol.

 
Lol I can relate to that!

The video was quite helpful with some slight oscillating problems I was having, but I already sort of understand everything else in the video. But I got your personal message and might have a look for him at the other forum.

His post on this contains all of the MATLAB code he used for the simulations at the end of the video. That might be more helpful. You can find it at:
http://forums.trossenrobotics.com/showthread.php?t=3812
Just work down the thread and you will see it.

If you'd like when I finish the project, will have to be done by the end of next month, I can post the .jar file or send you the source code.

I would really appreciate that! The math behind this is just above what I have learned (Calculus level but I've never covered this) and it have been tough just trying to decipher what is going on, much less put it into code.

I'm sorry I couldn't offer more help