1. Spatial Operator Algebra Review
Why Spatial Operator Algebra (SOA)?
Enables concise representation of the full-complexity of spatial kinematic and dynamic equations of multi-body systems.
Facilitates physical intuition and insight into relationships and approaches for developing algorithms.
Represents algorithmically efficient approaches for kinematic and dynamic computations.
References
G. Rodriguez, “Kalman Filtering, Smoothing and Recursive Robot Arm Forward and Inverse Dynamics,” IEEE Journal of Robotics and Automation, 3(6): , December 1987.
G. Rodriguez, K. Kreutz-Delgado, A. Jain, “A Spatial Operator Algebra for Manipulator Modeling and Control,” International Journal of Robotics Research, 10(4): , August 1991.
G. Rodriguez, A. Jain, K. Kreutz-Delgado, “Spatial Operator Algebra for Multibody System Dynamics,” Journal of the Astronautical Sciences, 40(1):27-50, January-March 1992.
G. Rodriguez, K. Kreutz-Delgado, “Spatial Operator Factorization and Inversion of the Manipulator Mass Matrix,” IEEE Trans. Robotics and Automation, 8(1):65-75, February 1992.
A. Jain, G. Rodriguez, “Linearization of Manipulator Dynamics Using Spatial Operators,” IEEE Trans. Systems, Man, and Cybernetics, 23(1): , January-February 1993.

2. Spatial Operator Algebra Overview
What is SOA?
Arose from recognition of analogies between the equations of force and motion of multi-body systems and the recursive difference equations used in Kalman filtering and signal processing.
Representation of kinematic and dynamic equations of multi-body systems with simple matrix equations.
Is applicable for force/velocity propagation equations and forward and inverse dynamics equations of multi-body systems.
Relevance of SOA to CLARAty
Desirable to implement generic algorithms for:
inverse kinematics - given a desired pose of a point on the system, what are the
corresponding joint values.
inverse dynamics - given a desired motion for the system, what are the corresponding
torques to cause the motion.

3. Example: Propagating Spatial Velocities across BodiesVi
hingei
bodyi
hingei-1
Vi-1
Vi+1
li, i-1
bodyi+1
li+1, i
bodyi-1
to base (inward)
to tip (outward)
li-1, i-2
hingei-2
Vi-2
hingei+1
bodyi+2
bodyi-2

4. Example: Propagating Spatial Velocities across Bodies
Definitions
wi x
wi y
wi z
ni x
ni y
ni z wi ni
Vi = = 1 spatial velocity
l(i +1,i) x
l(i +1,i) y
l(i +1,i) z
l (i +1,i) = 1 translation vector
Hi = 6 joint axis projection vector
hr = 0 for prismatic joint
hp = 0 for revolute joint
hr x hr y hr z hp x hp y hp z
qi -- scalar ith joint velocity
f(i +1,i) -- 66 transition matrix between base-bound nodes of bodies i+1 and i

5. Velocity at in-bound node of a body
Example: Propagating Spatial Velocities across Bodies
Velocity at in-bound node of a body
Vi = velocity due to bodyi+1 + velocity due to hingei
= Hi qi
wi+1
wi+1  l (i +1,i) + ni+1 *
l (i+1, i ) z l (i+1, i) y
- l (i +1,i) z l (i +1, i) x
l (i +1, i ) y - l (i +1, i ) x
But
~ *
wi+1  l (i +1,i) = wi = l (i+1, i) wi+1 So
Vi = Hi qi
= Hi qi = fi+1, i Vi Hi qi I
l (i +1,i) I
wi+1
ni+1 *
~ * ~ *
I l (i +1,i) I
wi+1
ni+1 * * *

6. Properties of transition matrix, f
Example: Propagating Spatial Velocities across Bodies
Properties of transition matrix, f
f(i+1,i) f(i,i-1) = f(i+1,i-1)
f(i+1,i) f(i,i-1) f(i-1,i-2) = f(i+1,i-2)
f(i-1,i-2) f(i,i-1) f(i+1,i) = f(i+1,i-2)
f(i,j) = f(j,i)
f(i,j) propagates spatial forces at point i to point j
f(i,j) propagates spatial velocities at point i to point j
f(i,i) = I -- the identity matrix * * * * -1 *

7. Propagating spatial velocities
Example: Propagating Spatial Velocities across Bodies
Propagating spatial velocities *
Vi = Hi qi
+ f(i+1,i) Hi+1 qi+1
f(i+1,i) f(i+2,i+1) Hi+2 qi+2 
+ f(i+1,i) f(i+2,i+1)  f(n-2,n-3) f(n-1,n-2) Hn-1 qn-1
+ f(i+1,i) f(i+2,i+1) f(n-2,n-3)  f(n-2,n-3) f(n-2,n-3) f(n-1,n-2) Hn qn
Vi = Hi qi + f(i+1,i) Hi+1 qi+1 + f(i+2,i) Hi+2 qi+2 +  + f(n-1,i) Hn-1 qn-1 + f(n,i) Hn qn * * * * * * * * * * * * * * * * * * * * * * * *

8. Algorithms for propagating spatial velocities
Example: Propagating Spatial Velocities across Bodies
Algorithms for propagating spatial velocities
Iteration
Summation
Vn+1 = 0
for (i = n; i > 0; i--)
Vi = f(i+1,i) Vi+1+ Hi qi
V0 = f(1,0) V1 n *
Vi =  f(j,i) Hj qj
V0 = f(1,0) V1 *
j = i * * * *

9. Spatial Operator Representation
Example: Propagating Spatial Velocities across Bodies
Spatial Operator Representation
I 
f(2,1) I 
f(3,1) I I 
f(n,1) f(n,2) f(n,3)  I * *
V = [Vi, Vi+1,  , Vn-1, Vn ] * * * * * * * *
H = diag[H1, H2 ,  , Hn-1, Hn]
F =      *
B = [f(1,0) , 0 ,  , 0, 0] * * *
V = F H q
V0 = B F H q
B F H = J SOA factorization of the Jacobian Matrix
J = H F B * * * * * * *

10. Spatial Operator Representation
Example: Inverse Dynamics
Spatial Operator Representation * *
V = F H q -- velocity propagation in outward direction
a = F [H q + a ] -- acceleration propagation in outward direction
f = F [M a + b ] -- force propagation in inward direction
T = H f -- projecting spatial forces along joint axes to get joint torques ·· * *

11. Algorithm Implementation
Example: Inverse Dynamics
Algorithm Implementation
for (i=n; i>0; i--) {
Vi = f(i+1,i) Vi Hi qi -- velocity propagation in outward direction
ai = f(i+1,i) a i+1 + Hi qi + ai -- acceleration propagation in outward direction }
for (i=1; i<n+1; i++) {
fi = f(i,i-1) fi Mi ai + bi -- force propagation in inward direction
Ti = Hi fi -- projecting spatial forces along joint axes to get
joint torques * * ·· * *