v 0 = 0 ; a 0 = − a g ; for i = 1 to N do , [ X J i , s i ] = jcalc ( j t y p e ( i ) , q i ) , i X λ ( i ) = X J i X T ( i ) , v i = i X λ ( i ) v λ ( i ) + s i q ˙ i , a i = i X λ ( i ) a λ ( i ) + s i q ¨ i + v i × s i q ˙ i , f i = I i a i + v i × ∗ I i v i ; end ; for i = N to 1 do , τ i = s i T f i , if λ ( i ) ≠ 0 then , f