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.
