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):624-639, 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):371-381, 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):239-248, January-February 1993.

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.

Example: Propagating Spatial Velocities across Bodies Vi 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

Example: Propagating Spatial Velocities across Bodies Definitions wi x wi y wi z ni x ni y ni z wi ni Vi = = -- 61 spatial velocity l(i +1,i) x l(i +1,i) y l(i +1,i) z l (i +1,i) = -- 31 translation vector Hi = -- 1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

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 · * 0 l (i+1, i ) z -l (i+1, i) y - l (i +1,i) z 0 l (i +1, i) x l (i +1, i ) y - l (i +1, i ) x 0 But ~ * wi+1  l (i +1,i) = wi+1 = l (i+1, i) wi+1 So Vi = + Hi qi = + Hi qi = fi+1, i Vi+1 + Hi qi I 0 l (i +1,i) I wi+1 ni+1 * · ~ * ~ * I l (i +1,i) 0 I wi+1 ni+1 · · * * *

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 *

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 * · * · * * * · * * * * * · * * * * * * · · * * · · · * * * * * *

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 * · * * *

Spatial Operator Representation Example: Propagating Spatial Velocities across Bodies Spatial Operator Representation I 0 0  0 f(2,1) I 0  0 f(3,1) I I  0 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 · * * * * * * *

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 ·· * *

Algorithm Implementation Example: Inverse Dynamics Algorithm Implementation for (i=n; i>0; i--) { Vi = f(i+1,i) Vi+1 + 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-1 + Mi ai + bi -- force propagation in inward direction Ti = Hi fi -- projecting spatial forces along joint axes to get joint torques · * * ·· * *