// Calculates the transformation matrix by multiplying all joint matrices
// End coordinates are stored in the properties class if the end coordinates are
// within the reachable space. An error message is produced if the end coordinates
// cannot be reached.
private void calcResult(Properties p) {
result = t1.multiply(t2);
result = result.multiply(t3);
result = result.multiply(t4);
result = result.multiply(t5);
result = result.multiply(t6);
double x = result.getValue(0, 3);
if (x < 0.00001 && x > -0.0001) x = 0.0;
double y = result.getValue(1, 3);
if (y < 0.00001 && y > -0.0001) y = 0.0;
double z = result.getValue(2, 3);
if (z < 0.00001 && z > -0.0001) z = 0.0;
// Stores new end coordinates in properties class if point can be reached
theta2 = p.getValueJoint2();
maxAngleJoint2 = (Math.acos(p.getValueLink1() / p.getValueLink2()) * 180 / Math.PI);
reachable = reachEndPoint();
if (reachable) {
p.setValueX(x);
p.setValueY(y);
p.setValueZ(z);
p.setCanReach(true);
} else {
p.setCanReach(false);
p.setErrorMessage(errorMessage);
}
}
// Constructor method
public ForwardKinematicsSolver(Properties p) {
result = new Matrix4x4();
t1 = new Matrix4x4(p.getValueJoint1(), -90, 0, p.getValueLink1());
t2 = new Matrix4x4(p.getValueJoint2(), 0, p.getValueLink2(), 0);
t3 = new Matrix4x4(p.getValueJoint3(), -90, p.getValueLink3(), offsetShoulder);
t4 = new Matrix4x4(p.getValueJoint4(), 90, offsetGripper, 0);
t5 = new Matrix4x4(p.getValueJoint5(), -90, 0, 0);
t6 = new Matrix4x4(p.getValueJoint6(), 0, 0, 0);
calcResult(p);
}
// If any of the six joint angles have changed the result matrix is calculated
// Any joint angle changes are stored in the transformation matrices
// Method returns true if any angle has been changed
public boolean calcEndCoordinates(Properties p) {
// indicates whether an angle changed
angleChanged = false;
// check if angle for each joint has changed. If yes, calculate the new matrix
if (t1.getTheta() != p.getValueJoint1()) {
t1.changeTheta(p.getValueJoint1());
angleChanged = true;
}
if (t2.getTheta() != p.getValueJoint2()) {
t2.changeTheta(p.getValueJoint2());
angleChanged = true;
}
if (t3.getTheta() != p.getValueJoint3()) {
t3.changeTheta(p.getValueJoint3());
angleChanged = true;
}
if (t4.getTheta() != p.getValueJoint4()) {
t4.changeTheta(p.getValueJoint4());
angleChanged = true;
}
if (t5.getTheta() != p.getValueJoint5()) {
t5.changeTheta(p.getValueJoint5());
angleChanged = true;
}
if (t6.getTheta() != p.getValueJoint6()) {
t6.changeTheta(p.getValueJoint6());
angleChanged = true;
}
if (angleChanged) calcResult(p);
return angleChanged;
}