4
\$\begingroup\$

I have an array of points in a 2d worldspace and I want to calculate which points are reachable by the end effector of a limb controlled with a CCDIK chain (with rotation limits applied).

  • It's a limb with 2 hinges (upper arm & lower arm)
  • The upper & lower arm have a fixed length
  • The 2 hinges have fixed rotation limits
  • This needs to be recalculated every update (continuously) as the limb is attached to a moving body
  • I work in Unity using C#

Crude Illustration of the problem: enter image description here

My idea sofar is to create a polygon collider along the outer edges of the reachable space, and then look if the points are within the bounds of that collider. But I have no idea if this is the smartest solution, or if so, how to achieve this.

How could I solve this issue?

\$\endgroup\$
2
\$\begingroup\$

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

| improve this answer | |
\$\endgroup\$
  • \$\begingroup\$ This answer pointed my in the right direction in an elegant mathematical way. There is however a way to be more efficient and take less steps to achieve the same result, which I added as a new separate answer \$\endgroup\$ – Edo van Royen Oct 7 '19 at 15:24
1
\$\begingroup\$

BONUS ANSWER: The question is only asking for, "Can the arm reach the point?" However, the next question would typically be, "What angles do I set each joint to so that the arm is touching the point?" My first answer describes the set up nicely, so using the terms of the answer, I will answer the followup question.

At this point, we know the absolute values of the angle Sang between (SE and Sdef) and the angle Eang between (SE and EH), but not the signed value.

TO FIND THE SIGNED VALUE OF THE ANGLES:

  1. Expand the 2D vectors into 3D vectors by added a zero in the Z component: e.g. SE3 = Vec3(SE.x, SE.y, 0.0f), Sdef3 = Vec3(Sdef.x, Sdef.y, 0.0f).
  2. Get the cross product SdefxSE of Sdef3 and SE3: SdefxSE = Sdef3 (cross) SE3.
  3. The sign of SdefxSE.z will tell you whether to use +Sang or -Sang depending on whether you are using a right-hand or left-hand coordinate system. You may need to flip the sign.
  4. Get the cross product SExEH of SE3 and EH3: SExEH = SE3 (cross) EH3.
  5. The sign of SExEH.z will tell you whether to use +Eang or -Eang depending on whether you are using a right-hand or left-hand coordinate system. You may need to flip the sign.
| improve this answer | |
\$\endgroup\$
0
\$\begingroup\$

Building on Gale's answer, I made the formula/math a little simpler, and I added my working C# code. I've built this in Unity, and used the same variables as Gale did.

SHMin

// 1. Get the max angle that E can turn (whichever way) in Radians = maxEAng

float maxEAng = Mathf.Max(Mathf.Abs(EHinge.min), Mathf.Abs(EHinge.max)) * Mathf.Deg2Rad;

// 2. Use the max angle of E to calculate the vector to H from E

Vector2 EHVec = new Vector2(Mathf.Cos(maxEAng), Mathf.Sin(maxEAng)) * EHlen;

// 3. Add the length of SE to the x of the vector towards the end effector to find the position of H

Vector2 H = new Vector2(EHVec.x + SElen, EHVec.y);

// 4. The min distance from S to H is the magnitude of the position of H, given that S is at 0,0

SHMin = H.magnitude;

Calculate which grip is in range

THis code loops through all the grips (points) and for each one checks if it is reachable, given the constraints of the arm.

// Set H equal to a grip in the list

for (int i = 4; i < gripArray.Length; i++)
    { Vector2 H= gripArray[i].position;

// Get SHlen: SHlen = length(H - S)

float SELen = (H - S).magnitude;

//Early Out: If (SE < minDistanceToE), then the point is too close for the arm to reach.

if (SE < minDistanceToE) {continue;}

// Early Out: If(SH > Slen + Elen), then point is beyond the reach of the arm

if (SH > Slen + Elen){continue;}

// Use law of cosines to find angle between SE and SH c ^ 2 = a ^ 2 + b ^ 2 − 2ab cos(C) // This will be Elen ^ 2 = Slen ^ 2 + SHlen ^ 2 - 2(Elen)(SHlen)cos(C) // Which solves to C = ACOS((Slen ^ 2 + SHlen ^ 2 - Elen ^ 2) / (2(Slen)(SHlen))

float C = Mathf.Acos(
(Mathf.Pow(Slen, 2) + Mathf.Pow(SHlen, 2) - Mathf.Pow(Elen, 2))
                / (2 * Slen * SHlen)
                );

// Create a vector representing the direction of SE respective to SH // vecA = (cos(C), sin(C))

Vector2 vecA = new Vector2(Mathf.Cos(C), Mathf.Sin(C));

// Normalize SHnorm = normalize(SH)

Vector2 SENorm = (H -S).normalized;

// What are the angles for the direction of SE respective to SH + SH respective to the (0,-1) rotation limit axis

var SEVecAngle = Vector2.SignedAngle(new Vector2(1, 0), vecA) ;
var SHAng= Vector2.SignedAngle(new Vector2(0, -1), SHnorm);
var combinedAngle = SEVecAngle + SHAng;

// Final Out: If(combinedAngle > maxSAngle or < minSAngle), where the max and min angle is measured from rotation axis (0,-1) then arm can't reach the point.

if (combinedAngle > maxSAngle || combinedAngle < minSAngle) {continue;}

Debug.Log(“Grip “ + [I] + “ is reachable!”

| improve this answer | |
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.