IK chain constraints, FABRIK algorithm

I have read several algorithms for Inverse Kinematics and Joint constraints. I found an algorithm FABRIK: A fast, iterative solver for the Inverse Kinematics problem by Andreas Aristidou . In case if you are unable to access the full paper, please download here.

FABRIK seems promising in comparison with CCD, Jacobian methods etc.

This algorithm describes an approach for joint constraints in section 4.3 , pseudo-code given in Algorithm 2, 3 in the paper. There are two kinds of constraints discussed: orientational(1 DoF), rotational(2 DoFs). Orientational constraints are easy, but rotational constraints are not. Rotational constraint for a joint is explained using a irregular conic shape. I'm using C# scripting in Unity 3D for the implementation.

I have worked as well as goggled and found the following links, but could not find any information about constraints:
googlecode

github

unitypackage

I could not understand how to draw a irregular cone and 3D to 2D mapping of ellipsoid concept discussed in the paper. According to the psuedo-code, I did the the following.



The following is what I have done so far:

Code (csharp):

1. void Algorithm3(GameObject objN, GameObject objN_1, Vector3 targetPos, Quaternion targetOri, float[] limitAngles)
2. {
3.     // ObjN and ObjN_1 are the arm joints of a 3D human model.
4.     Quaternion oribtw = Quaternion.Inverse(objN.transform.rotation) * objN_1.transform.rotation;
5.     Vector3 P1online = objN_1.transform.position; Vector3 P2online = objN.transform.position;
6.     Vector3 line = P1online - P2online; // direction
7.     //project 'targetPos' on 'line' this results in a point 'O'.
8.     Vector3 pointO = Vector3.Project( (targetPos-P1online), (P1online-P2online) ) + P1online;      
9.     // calculate distance 'S' between 'O' and 'objN'
10.     float distance_S = Vector3.Distance(pointO, objN_1);
11.     float[] distances = new float[4];
12.     for( int i=0; i< 4; i++){
13.      distances [i] = distance_S * Math.tan(limitAngles[i]); //limitAngles = -20, 40, -30, 75
14.     }
15. //step3.4: Map the target ( rotate and translate) in such a way that 'pointO' is now located at the axis origin and oriented according to the x and y-axis.
16. // step3.5: Find in which quadrant the target belongs.
17. // step3.6 ---> step3.14
18. }

Thanks for any help in implementation anyone might can provide.

 

Hello @harinath.
At first, please excuse my awkward English.

I'm also finding the method to constrain the joints in FABRIK algorithm to make a finger IK.
So far, I made progress to make a polygon from angles(Theta1 to Theta4), and it seems I can extract 2D figure.



However, I've wondered how to make a figure like (b), I have an idea but haven't tried yet.
The idea is using Quaternion.Slerp to make curves like (b).

BTW, even if I make the figure, there is another problem, I have to calculate some point is inside or outside the figure.
If the figure is a polygon, there is a theory called Crossing Number Method.
I tested this for a polygon and it works well but, I have no idea for (b).

I'm still trying to make progress.
If you have any idea, please let me know.

Thank you.
And here's my codes which based on this thread.

FABRIK.cs

Code (csharp):

1. using UnityEngine;
2. using System.Collections;
3.  
4. /**
5.  * FABRIK Solver based on paper found here - www.andreasaristidou.com/publications/FABRIK.pdf  
6.  *
7.  * http://forum.unity3d.com/threads/187838-INVERSE-KINEMATICS-Scripting-Tutorial-Searching?p=1283005&viewfull=1#post1283005
8. */
9.  
10. public class FABRIK : MonoBehaviour
11. {
12.     public int maxSolverIterations = 20; // 15 iterations is average solve time
13.     public float solveAccuracy = 0.001f;
14.    
15.     public IKChain myChain;
16.    
17.     void Start()
18.     {
19.         this.myChain.Init();
20.     }
21.    
22.     void Update()
23.     {
24.         if (this.myChain.target != null)
25.         {
26.             this.Solve(this.myChain);
27.         }
28.     }
29.    
30.     void OnDrawGizmos()
31.     {
32.         this.myChain.DebugDraw();
33.     }
34.    
35.     void Solve(IKChain chain)
36.     {
37.         var joints = chain.joints;
38.         if (joints.Length < 2)
39.             return;
40.        
41.         var rootToTargetDist = Vector3.Distance(joints[0].position, chain.target.position);
42.         var lambda = 0f;
43.        
44.         // Target unreachable
45.        
46.         if (rootToTargetDist > chain.length)
47.         {
48.             for (int i = 0; i < joints.Length - 1; i++)
49.             {
50.                 lambda = chain.segLengths[i] / Vector3.Distance(joints[i].position, chain.target.position);
51.                 joints[i+1].position = (1 - lambda) * joints[i].position + lambda * chain.target.position;
52.             }
53.         }
54.         else // Target within reach
55.         {
56.             chain.Reset();
57.            
58.             var rootInitial = joints[0].position;
59.             var tries = 0;
60.             var targetDelta = Vector3.Distance(joints[joints.Length-1].position, chain.target.position);
61.            
62.             while (targetDelta > this.solveAccuracy  tries < this.maxSolverIterations)
63.             {
64.                 // Forward reaching phase
65.                
66.                 joints[joints.Length-1].position = chain.target.position;
67.                
68.                 for (int i = joints.Length - 2; i > 0; i--)
69.                 {
70.                     lambda = chain.segLengths[i] / Vector3.Distance(joints[i+1].position, joints[i].position);
71.                     var pos = (1 - lambda) * joints[i+1].position + lambda * joints[i].position;
72.                     joints[i].position = pos;
73.                     joints[i].position = this.Constraints(joints[i+1], joints[i]);
74.                 }
75.                
76.                 // Backward reaching phase
77.                
78.                 joints[0].position = rootInitial;
79.                
80.                 for (int i = 0; i < joints.Length - 1; i++)
81.                 {
82.                     lambda = chain.segLengths[i] / Vector3.Distance(joints[i+1].position, joints[i].position);
83.                     var pos = (1 - lambda) * joints[i].position + lambda * joints[i+1].position;
84.                     joints[i+1].position = pos;
85.                     joints[i+1].position = this.Constraints(joints[i], joints[i+1]);
86.                 }
87.  
88.                 targetDelta = Vector3.Distance(joints[joints.Length-1].position, chain.target.position);
89.                 tries++;
90.             }
91.         }
92.     }
93.    
94.     Vector3 Constraints(IKJoint j, IKJoint j_1)
95.     {
96.         return j_1.position;
97.     }
98. }
99.  

IKChain.cs

Code (csharp):

1. using UnityEngine;
2. using System.Collections;
3.  
4. [System.Serializable]
5. public class IKChain
6. {
7.     public Transform target;
8.     public IKJoint[] joints;
9.    
10.     [System.NonSerialized]
11.     public float[] segLengths;
12.    
13.     [System.NonSerialized]
14.     public float length;
15.    
16.     private Vector3[] localPositions; //**temp
17.     private Quaternion[] localRotations; //**temp
18.    
19.     public bool Inited
20.     {
21.         get{ return this.segLengths != null; }
22.     }
23.    
24.     public void Init()
25.     {
26.         this.segLengths = new float[joints.Length];
27.        
28.         for (int i = 0; i < this.joints.Length - 1; i++)
29.         {
30.             float dist = (this.joints[i].position - this.joints[i+1].position).magnitude;
31.             {
32.                 this.segLengths[i] = dist;
33.                 this.length += dist;
34.             }
35.         }
36.        
37.         this.localPositions = new Vector3[joints.Length];
38.         this.localRotations = new Quaternion[joints.Length];
39.        
40.         for (int i = 0; i < this.joints.Length; i++)
41.         {
42.             this.localPositions[i] = this.joints[i].localPosition;
43.             this.localRotations[i] = this.joints[i].localRotation;
44.         }
45.     }
46.    
47.     public void Reset()
48.     {
49.         for (int i = 0; i < this.joints.Length; i++)
50.         {
51.             this.joints[i].localPosition = this.localPositions[i];
52.             this.joints[i].localRotation = this.localRotations[i];
53.         }
54.     }
55.    
56.     public void DebugDraw()
57.     {
58.         if (!this.Inited)
59.             this.Init();
60.        
61.         for (int i = 0; i < this.joints.Length - 1; i++)
62.         {
63.             Gizmos.color = Color.cyan;
64.             Gizmos.DrawLine(this.joints[i].position, this.joints[i+1].position);
65.            
66.             var con = this.joints[i].constraints;
67.             if (con == null)
68.                 continue;
69.            
70.             var len = this.segLengths[i];
71.             var a = this.joints[i].position;
72.             var b = this.joints[i+1].position;
73.             var ar = this.joints[i].rotation;
74.             var br = this.joints[i+1].rotation;
75.            
76.             var line1 = ar * con.upDir * 0.03f;
77.             var line2 = -line1;
78.             var line3 = br * con.rightDir * 0.03f;
79.             var line4 = -line3;
80.             Gizmos.color = Color.gray;
81.             Gizmos.DrawLine(b, b + line1);
82.             Gizmos.DrawLine(b, b + line2);
83.             Gizmos.DrawLine(b, b + line3);
84.             Gizmos.DrawLine(b, b + line4);
85.            
86.             var upRad = Mathf.Deg2Rad * con.upDegree;
87.             var downRad = Mathf.Deg2Rad * con.downDegree;
88.             var leftRad = Mathf.Deg2Rad * con.leftDegree;
89.             var rightRad = Mathf.Deg2Rad * con.rightDegree;
90.             var line5 = len * (ar * new Vector3(-Mathf.Cos(upRad), Mathf.Sin(upRad), 0));
91.             var line6 = len * (ar * new Vector3(-Mathf.Cos(downRad), -Mathf.Sin(downRad), 0));
92.             var line7 = len * (ar * new Vector3(-Mathf.Cos(leftRad), 0, Mathf.Sin(leftRad)));
93.             var line8 = len * (ar * new Vector3(-Mathf.Cos(rightRad), 0, -Mathf.Sin(rightRad)));
94.             Gizmos.color = Color.blue;
95.             Gizmos.DrawLine(a + line5, a + line6);
96.             Gizmos.DrawLine(a + line7, a + line8);
97.  
98.             Gizmos.color = Color.yellow;
99.             Gizmos.DrawLine(a, a + line5);
100.             Gizmos.DrawLine(a, a + line6);
101.             Gizmos.DrawLine(a, a + line7);
102.             Gizmos.DrawLine(a, a + line8);
103.  
104.             Gizmos.color = Color.green;
105.             Gizmos.DrawLine(a + line5, a + line8);
106.             Gizmos.DrawLine(a + line8, a + line6);
107.             Gizmos.DrawLine(a + line6, a + line7);
108.             Gizmos.DrawLine(a + line7, a + line5);
109.         }
110.     }
111. }

IKJoint.cs

Code (csharp):

1. using UnityEngine;
2. using System.Collections;
3.  
4. [System.Serializable]
5. public class IKConstraints
6. {
7.     public float upDegree = 90;
8.     public float downDegree = 5;
9.     public float leftDegree = 0;
10.     public float rightDegree = 0;
11.     public Vector3 forwardDir = new Vector3(-1,0,0);
12.     public Vector3 rightDir = new Vector3(0,0,1);
13.     public Vector3 upDir = new Vector3(0,1,0);
14. };
15.  
16. [System.Serializable]
17. public class IKJoint
18. {
19.     public Transform root;
20.     public IKConstraints constraints;
21.    
22.     public Vector3 position{
23.         get{ return this.root.position; }
24.         set{ this.root.position = value; }
25.     }
26.    
27.     public Quaternion rotation{
28.         get{ return this.root.rotation; }
29.         set{ this.root.rotation = value; }
30.     }
31.    
32.     public Vector3 localPosition{
33.         get{ return this.root.localPosition; }
34.         set{ this.root.localPosition = value; }
35.     }
36.    
37.     public Quaternion localRotation{
38.         get{ return this.root.localRotation; }
39.         set{ this.root.localRotation = value; }
40.     }
41.    
42.     public static implicit operator Transform(IKJoint joint){
43.         return joint.root;
44.     }
45.    
46.     public static implicit operator IKJoint(Transform joint){
47.         return new IKJoint(joint);
48.     }
49.    
50.     public IKJoint(){
51.     }
52.    
53.     public IKJoint(Transform t){
54.         this.root = t;
55.     }
56. };
57.  

 

Still trying to implement constraint for joints...
So, what discribed in first message is only one type of constraints could be use. There some other constraint type/

For example spherical joint type (simple axis and angle ), hinge joint (1DOF), reach cone ... and other what you would like to implement. Searching web for working solution yo can try will not give a result.

Trying to implement spherical joint don't give a good result for me. In attach is current version, with constraint solver for forward and back stage. The result of this solver is pure, seems like it working, but the result is awful...

Maybe anyone interested in this theme will help with advise or review of mistakes...

My idea for FWD: when you get the limited joint you look for angle beetwen axis and [i+1], if it larger then limit angle, you get a difference, and translate on this angle around [i+1] joint.

For back: when you get limited you simply calculate angle (like in forward) and translate [i+1] around to get [i+1] in angle of "rotor".

It seems that its enough of back stage costraint apply

But the back stage with my algorythm don't give stable result.

 

Just have made a better version, but still have some problems, this version have angle (ball socket) constraint on back stage + singularity check. Not smooth translate, and in some cases very bad solve, i think the problem is in my week quaternion knowledge..
Have some problems, and need your help!

sry about trash at the scene