I'm trying to calculate the inverse kinematic for an 6 dof manipulator.
Task:
A target point $p_{target} = (x,y,z)^T$ and the orientation $o_{target} = (a, b, c)^T$ are given and I want to get the angle configuration $q = (q_1, q_2, q_3, q_4, q_5, q_6)^T$ for my robot.
Method:
For that I try to implement the Jacobian method (with the transposed jacobian matrix) with this guide and followed the pseudocode at slide 26. But instead using the pseudoinverse of the Jacobian matrix I used the transposed one.
I'll try to compute the Jacobian matrix numerically and analytically, but didn't get a solution (endless loop) for any of them. Here's how I retrieve the Jacobian:
Numerically:
private void calculateMatrixNumerically() { var deltaTheta = 0.01; var newAxisConfiguration = new AxisConfiguration( this.currentAxisConfiguration.joints[0] + deltaTheta, this.currentAxisConfiguration.joints[1] + deltaTheta, this.currentAxisConfiguration.joints[2] + deltaTheta, this.currentAxisConfiguration.joints[3] + deltaTheta, this.currentAxisConfiguration.joints[4] + deltaTheta, this.currentAxisConfiguration.joints[5] + deltaTheta ); var ePos = this.kinematic.CalculateDirectKinematic(newAxisConfiguration); for (var column = 0; column < this.Columns; column++) { this.M[0, column] = (this.currentPos.Point.X - ePos.Point.X) / deltaTheta; this.M[1, column] = (this.currentPos.Point.Y - ePos.Point.Y) / deltaTheta; this.M[2, column] = (this.currentPos.Point.Z - ePos.Point.Z) / deltaTheta; this.M[3, column] = (this.currentPos.Orientations[0].A - ePos.Orientations[0].A) / deltaTheta; this.M[4, column] = (this.currentPos.Orientations[0].B - ePos.Orientations[0].B) / deltaTheta; this.M[5, column] = (this.currentPos.Orientations[0].C - ePos.Orientations[0].C) / deltaTheta; } }Analytically:
private void calculateMatrixAnalytically() { var peMatrix = calculateJointPositions(); var zMatrix = calculateZ(); for (var column = 0; column < this.Columns; column++) { double[] p_p = new double[3]; for(var row = 0; row < zMatrix.Rows; row++) { p_p[row] = peMatrix.M[row, this.Columns-1] - peMatrix.M[row, column]; } this.M[0, column] = zMatrix.M[1, column] * p_p[2] - zMatrix.M[2, column] * p_p[1]; this.M[1, column] = zMatrix.M[2, column] * p_p[0] - zMatrix.M[0, column] * p_p[2]; this.M[2, column] = zMatrix.M[0, column] * p_p[1] - zMatrix.M[1, column] * p_p[0]; this.M[3, column] = zMatrix.M[0, column]; this.M[4, column] = zMatrix.M[1, column]; this.M[5, column] = zMatrix.M[2, column]; } } /// <summary> /// Calculate the positions of every joint. /// </summary> /// <returns>The Matrix with the joint coordinate for each joint.</returns> private Matrix calculateJointPositions() { Matrix jointPositions = new Matrix(3,6); Position pos; for (var joint= 0; joint< this.currentAxisConfiguration.joints.Count(); joint++) { pos = this.kinematic.CalculateDirectKinematic(this.currentAxisConfiguration, joint); jointPositions.M[0, joint] = pos.Point.X; jointPositions.M[1, joint] = pos.Point.Y; jointPositions.M[2, joint] = pos.Point.Z; } return jointPositions; } private Matrix calculateZ() { // (z0^T-z1^T-z2^T-...-z6^T) var ksEnd = Kinematics.TranslateRobotToWorld(); var zMatrix = new Matrix(3, 6); for (var column = 0; column < this.currentAxisConfiguration.joints.Count(); column++) { for (var row = 0; row < zMatrix.Rows; row++) zMatrix.M[row, column] = Math.Round(ksEnd.M[row, 2], 7); ksEnd = ksEnd.Multiply( Kinematics.TranslateCoordinateSystem( Robo.theta[column] + this.currentAxisConfiguration.joints[column], Robo.d[column], Robo.alpha[column], Robo.a[column]) ); } return zMatrix; }
Here is the implementation of the Pseudocode:
do
{
jacob = JacobiMatrix.GetJacobi(currentPosition, currentAxisConfiguration);
jacobiTranspose = jacob.getTransposeMatrix();
// deltaE = (x2-x1, y2-y1, z2-z1, a2-a1, b2-b1, c2-c1)
deltaE = Position
.GetDistanceVector(currentPosition, targetPosition);
deltaThetas = jacobiTranspose.Multiply(deltaE).
Scale(beta);
for (var axis = 0; axis < deltaThetas.Rows; axis++ )
currentAxisConfiguration.joints[axis] += deltaThetas.M[axis, 0];
currentPosition = this.CalculateDirectKinematic(currentAxisConfiguration);
} while (Math.Abs(Position.Distance(currentPosition, targetPosition)) > epsilon);
where $beta = 0.5$ and $epsilon = 0.0001$
Problems:
The transformation matrice should be fine, because it behaves good for the forward kinematic. In my opinion the Jacobian matrix must be somehow wrong. I'm not sure if it is correct how I put the orientation data in the numerical calculation. For the analytical computation I didn't have any clue what could be wrong. I would be thankful for any advice. An explicit example for calculating the Jacobian would also be very helpful.
