コード例 #1
0
 // 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);
 }
コード例 #2
0
  // 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;
  }