// 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);
 }