Fundamentals of robotic mechanical systems_ theory, methods, and algorithms-Springer (2003)

158 4. Kinetostatics of Simple Robotic Manipulators represented in the same coordinate frame, say, the first one. We then have the expansion below: J˙ θ˙ = θ˙1 0 + θ˙2 e˙ 2 + · · · + θ˙n e˙ n (4.92) u˙ 1 u˙ 2 u˙ n The right-hand side of eq.(4.92) is computed recursively as described below in five steps, the number of operations required being included at the end of each step. 1. Compute { [ ωi ]i }1n: [ ω1 ]1 ← θ˙1[ e1 ]1 For i = 1 to n − 1 do [ ωi+1 ]i+1 ← θ˙i+1[ ei+1 ]i+1 + QTi [ ωi ]i enddo 8(n − 1) M & 5(n − 1) A 2. Compute { [ e˙ i ]i }1n: 0M & 0A [ e˙ 1 ]1 ← [ 0 ]1 For i = 2 to n do [ e˙ i ]i ← [ ωi × ei ]i enddo 3. Compute { [ r˙ i ]i }1n: [ r˙ n ]n ← [ ωn × an ]n For i = n − 1 to 1 do [ r˙ i ]i ← [ ωi × ai ]i + Qi[ r˙ i+1 ]i+1 enddo (14n − 8)M & (10n − 7)A 4. Compute { [ u˙ i ]i }n1 using the expression appearing in eq.(4.89c): [ u˙ 1 ]1 ← [ e1 × r˙ 1 ]1 For i = 2 to n do [ u˙ i ]i ← [ e˙ i × ri + ei × r˙i ]i enddo 4(n − 1) M & 3(n − 1) A TLFeBOOK