PDA

View Full Version : [Question(s)] Use of Jacobian

Boaxy
03-06-2010, 09:45 AM
Hey,
I have recently been working on a project that I've been having some problems with, and I was referred here by Parallax at the societyofrobots forum, after showing me a video made by a user here, WGhost9.

I have recently been developing a 3D robotic arm using the Java3D API.
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, non-planar cylindrical and scara.
To do this, I am trying to make use of the Jacobian and it's inverse.

The code I have to do this in the 2-link planar robot 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

The thetas are then passed into methods to rotate the associated joints.

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 problem is that the robot seems to move smoothly, but doesn't move to the point specified. It 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 Partial Derivatives out. My Scara partial derivatives are definitely correct.

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.