Skip to main content
There was an error in notation. Had the formula in the wrong order.
Source Link

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Hand (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(ElenSHlen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(ElenSHlen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Hand (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(Elen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(Elen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Hand (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(SHlen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(SHlen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Typo
Source Link

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and ElbowHand (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(Elen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(Elen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Elbow (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(Elen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(Elen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Hand (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(Elen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(Elen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram

Source Link

Given that the arm exists in a 2D plane, the joints are hinge joints and not ball and socket joints, and the rotation arc for each joint is +/- maxangle:

Let H = the position of the end effector.

Let S = the position of the "shoulder" joint.

Let Slen = the length from the shoulder to the elbow.

Let Sdef = a normal vector representing the default orientation of S.

Let E = the position of the "elbow".

Let Elen = the length from the elbow to the hand.

Let SHMin = the minimum possible separation of the Shoulder and Elbow (this can be pre-calculated).

TO FIND SHMin: Let Sdef = Vec(1, 0) (i.e. yaw set to zero)

  1. Set the vector SE from S to E: SE = Vec(cos(Smaxang), sin(Smaxang)) * Slen.
  2. Add SE to S to get E: E = S + SE.
  3. Add Smaxang to Emaxang to get SEangle: SEangle = Smaxang + Emaxang.
  4. Set the vector EH from E to H: EH = Vec(cos(SEangle), sin(SEangle)) * Elen.
  5. Add EH to E to get H: H = E + EH.
  6. Get the length of SH for Emaxang: SHlenemax = length(H-S).
  7. Repeat 3-6 for -1 * Emaxang to get SHlenemin.
  8. SHMin is the lesser of SHlenemin and SHlenemax.

TO DETERMINE IF POINT IS REACHABLE:

  1. Set H equal to a position in the list.
  2. Get the distance between S and H: SHlen = length(H-S).
  3. Early Out: If (SHlen > Slen + Elen), then point is beyond the reach of the arm.
  4. Early Out: If (SHlen < SHMin), then the point is too close for the arm to reach.
  5. You now have the length of all three sides of a triangle. Use law of cosines to find angle between S and SH: c^2 = a^2 + b^2 − 2ab cos(C). This will be Elen^2 = Slen^2 + SHlen^2 - 2(Slen)(Elen)cos(C) which solves to C = ACOS((Slen^2 + SHlen^2 - Elen^2) / (2(Slen)(Elen)).
  6. Create a normalized vector representing the direction of SE: VecA = (cos(C), sin(C)). SHnorm = normalize(SH). SEnorm = normalize(VecA + SHnorm).
  7. Get the angle Sang between SEnorm and Sdef: Sang = ACOS(SEnorm (dot) Sdef).
  8. Early Out: If (Sang > Sangmax), then arm can't reach the point.
  9. Get the position of E using SHnorm, Slen, and S: E = S + (SHnorm * Slen).
  10. Get the normalized vector EHnorm from E to H: EHnorm = normalize(H-E).
  11. Get the angle Eang between EHnorm and SHnorm: Eang = ACOS(EHnorm (dot) SHnorm).
  12. Final Out: If (Eang > Eangmax), then arm can't reach the point.

Loltastic Diagram