Dev:Source/GameEngine/RobotIKSolver/Algorithm

iTaSC algorithm

This page explains the iTaSC algorithm principles. Let's take a typical robotic situation:

Constraint geometrical loop

In this picture, base is the robot local reference, effector is the robot end effector on which a constraint is defined, target is the target for the constraint. It is always possible to build a transformation loop going from the world reference to the robot base reference, then the end effector, the target and finally back to the world reference.

For each segment of the loop, a transformation matrix can be defined, and the multiplication of all these matrices is the Identity as the loop is closed. For each matrix, it is possible to define a set of coordinates that define the transformation entirely.

For the transformation from the world reference to the robot base reference, we define the coordinates Xu that correspond to the robot base position and orientation. It can be fixed coordinates or variable if the robot base is moving. Xu stand for 'unconstrained coordinates' since the movements of the robot base is an external input outside the control of the algorithm. Similarly, the transformation from the target to the world reference is given by another set of unconstrained coordinates as the target can move freely. By grouping all the unconstrained coordinates we get the Xu vector. It is possible to derive the Xu coordinates from the objects position and orientation in Blender.

The transformation from the robot base to the end effector is unambiguously defined by the set of joints values that we group in vector q. The Forward Kinematics of the robot is by definition the function that links the end effector local reference to the vector q.

The gap between the end effector and the target is directly related to the constraint that we want to enforce on the end effector. Depending on the constraint, we may want to:

• bring the end effector at the position of the target (CopyPose)
• give the end effector the same position and orientation than the target (CopyPose+rot)
• bring the end effector at a given distance to the target (Distance)

This list corresponds to the constraints that are currently supported in iTaSC but it can easily be extended with other type of constraints.

The central idea of iTaSC is to define a virtual armature that starts from the end effector and terminates exactly at the target position and orientation. We give this virtual armature a very specific structure:

• it must have exactly 6 degrees of freedom and they must be chosen such that there is no restriction on the forward kinematics (any cartesian coordinates X/Y/Z/RotX/RotY/RotZ at the end of the virtual armature can be matched).
• We define the joints such that one or more of them have a direct significance with regards to the constraint.
  

Example: for the distance constraint we define 5 pure rotational joints with no offset and 1 translational joint that bridges the gap between the end effector and the target. The value of this joint is the distance between the end effector and the target. The Distance contraint now consits in imposing a specific value to that joint.

We group the 6 coordinates of this constraint armature in a vector that we call Xf, the feature coordinates.

With this in mind, we can now defined the equations of the system:

The output equation links Xf and q with the output values:

 1:  f(q,Xf) = y

where y if the vector of values that we want to control. The fact that q is included in this equation means that we give the possibility to control directly the joints of the armature. From the definition of Xf, f is a simple function. We derive it over time to get the linear approximation of this equation:

 2:  Cq.q' + Cf.Xf' = y'

where Cq and Cf are the selection matrix: the coefficients are mostly 0 except a few 1s to select the coordinates that we want to control.

The loop equation represents the fact that we have closed the geometrical loop by construction:

 3:  l(q,Xf,Xu) = 0

and it's derivative version:

 4:  Jq.q'+Jf.Xf'+Ju.Xu' = 0

where Jq is the Jacobian of the armature, Jf the Jacobian of the constraint virtual armature and Ju the Jacobian of the unconstrained references (Ju takes a simple fixed form).

By construction, Jf is a 6x6 matrix and is invertible. We can extract Xf' from equation 4. :

 5:  Xf' = -Jf^.Ju.Xu'-Jf^.Jq.q'

where Jf^ represents the inverse of Jf.

By replacing Xf' in equation 2 and by grouping the terms, we get:

 6:  (Cq-Cf.Jf^.Jq).q' = y' + Cf.Jf^.Ju.Xu'

If we call A=Cq-Cf.Jf^.Jq and B=Cf.Jf^.Ju we get the modified Jacobian equation:

 7:  A.q' = y'+B.Xu'

You will notice that equation 7 has the same form than the traditional Jacobian equation except that y' can represent many different things than just the gap between the end effector and the target. The term B.Xu' allows iTaSC to anticipate the target movements and improves greatly the precision of the tracking while keeping the number of iterations low.

To provide long term convergence on the desired output values, we compute y' as follow:

 8: y' = yd' + K(yd-y)

where yd and yd' are respectively the desired value and desired velocity of the output vector and K is a feedback coefficient. The current implementation of iTaSC uses a global feedback coefficient K for all constraints. For some constraints, the desired value is programmable (Distance and Joint rotation), for others, it is set to a fixed value (yd=yd'=0 for CopyPose).

The resolution of equation 7 is done by computing the pseudo inverse of A as usual:

 9: q' = A#.(y'+B.Xu')

It is possible to extend this method to multiple constraints and even multiple armatures interacting via constraints (supported in iTaSC but not in Blender yet). The A and B matrices increase in size but can always be computed in a generic manner.