Robot Mechanics

Dr. Robert L. Williams II

Mechanical Engineering
Ohio University
NotesBook Supplement for
ME 4290/5290 Mechanics and Control of Robotic Manipulators
2015 Dr. Bob Productions
williar4@ohio.edu
people.ohio.edu/williar4

4-dof SCARA Serial Robot

6-dof Stewart Platform Parallel Robot

These notes supplement the ME 4290/5290 NotesBook by Dr. Bob

This document presents supplemental notes to accompany the ME 4290/5290 NotesBook. The
outline given in the Table of Contents dovetails with and augments the ME 4290/5290 NotesBook
outline and thus is incomplete here.

ME 4290/5290 Supplement Table of Contents

2. SERIAL MANIPULATOR KINEMATICS ...................................................................................... 3
2.7 INVERSE POSE KINEMATICS ............................................................................................................ 3
2.7.2 Planar 3R Robot IPK Solution ................................................................................................... 3
2.7.4 Piepers Solution Method ......................................................................................................... 11
2.7.5 Spatial 8R NASA AAI ARMII Robot IPK Solution ................................................................... 13
2.8 TRAJECTORY GENERATION ........................................................................................................... 18
2.8.4 Single Fourth-Order Polynomial with a Via Point .................................................................. 18
2.9 VELOCITY KINEMATICS ................................................................................................................. 22
2.9.3 Jacobian Matrices ................................................................................................................... 22
2.10 ACCELERATION KINEMATICS ..................................................................................................... 24
4. PARALLEL ROBOTS ...................................................................................................................... 29
4.3 PLANAR 3-RPR MANIPULATOR ..................................................................................................... 29
4.3.1 Planar 3-RPR Manipulator Inverse Pose Kinematics ............................................................. 29
4.3.2 Planar 3-RPR Manipulator Forward Pose Kinematics ........................................................... 31
4.3.3 Planar 3-RPR Manipulator Velocity Kinematics ..................................................................... 33
4.4 NEWTON-RAPHSON METHOD ........................................................................................................ 37
4.5 PARALLEL MANIPULATOR WORKSPACE....................................................................................... 41
4.6 PLANAR 2-DOF FIVE-BAR PARALLEL ROBOT ............................................................................... 42
4.6.1 Pose Kinematics ....................................................................................................................... 43
4.6.2 Velocity Kinematics .................................................................................................................. 48
4.6.3 Acceleration Kinematics .......................................................................................................... 51
4.6.4 Inverse Dynamics ..................................................................................................................... 54
4.7 NIST ROBOCRANE CABLE-SUSPENDED PARALLEL ROBOT ........................................................ 61
4.8 DELTA PARALLEL ROBOT INVERSE AND FORWARD KINEMATICS ............................................... 71
4.9 STEWART PLATFORM ..................................................................................................................... 72
5. MANIPULATOR DYNAMICS ........................................................................................................ 73
5.1
5.2
5.3
5.4
5.5
5.6

INERTIA TENSOR (MASS DISTRIBUTION)....................................................................................... 74

NEWTON-EULER RECURSIVE ALGORITHM ................................................................................... 75
LAGRANGE-EULER ENERGY METHOD .......................................................................................... 77
SIMPLE DYNAMICS EXAMPLE ........................................................................................................ 78
STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS .............................................................. 83
ROBOT DYNAMICS EXAMPLE......................................................................................................... 84

6. ROBOT CONTROL ARCHITECTURES ...................................................................................... 88

6.1
6.2
6.3
6.4
6.5
6.6

INVERSE POSE CONTROL ARCHITECTURE .................................................................................... 88

INVERSE VELOCITY (RESOLVED-RATE) CONTROL ARCHITECTURE ........................................... 88
INVERSE DYNAMICS CONTROL ARCHITECTURE ........................................................................... 89
NASA LANGLEY TELEROBOTICS RESOLVED-RATE CONTROL ARCHITECTURE ........................ 90
NATURALLY-TRANSITIONING RATE-TO-FORCE CONTROLLER ARCHITECTURE ........................ 92
SINGLE JOINT CONTROL ................................................................................................................ 93

2. Serial Manipulator Kinematics

2.7 Inverse Pose Kinematics
2.7.2 Planar 3R Robot IPK Solution
Tangent half-angle substitution derivation
In this subsection we first derive the tangent
analytical/trigonometric method. Defining parameter t to be:

half-angle

substitution

using

an

t tan
2

i.e. the tangent of half of the unknown angle , we need to derive cos and sin as functions of
parameter t. This derivation requires the trigonometric sum of angles formulae.
cos( a b ) cos a cos b sin a sin b
sin( a b ) sin a cos b cos a sin b

To derive the cos term as a function of t, we start with:

cos cos
2 2

The cosine sum of angles formula yields:

cos cos 2 sin 2
2
2

Multiplying by a 1, i.e. cos 2 over itself yields:
2


cos 2 sin 2
2
2 cos 2 1 tan 2 cos2
cos




2
2
2
cos2
2

The cosine squared term can be divided by another 1, i.e. cos 2 sin 2 1 .
2
2

cos 2

cos 1 tan 2
2 cos 2 sin 2

2
2

Dividing top and bottom by cos 2 yields:

2

cos 1 tan 2
2 1 tan 2

Remembering the earlier definition for t, this result is the first derivation we need, i.e.:
cos

1 t2
1 t2

To derive the sin term as a function of t, we start with:

sin sin
2 2

The sine sum of angles formula yields:

sin sin cos cos sin 2 sin cos
2
2
2
2
2
2

Multiplying top and bottom by cosine yields:

sin
2 cos 2 2 tan cos 2
sin 2



2
2

2
cos
2
From the first derivation we learned:
1

cos 2
2 1 tan 2

2

Substituting this term yields:

sin 2 tan
2 1 tan 2

Remembering the earlier definition for t, this result is the second derivation we need, i.e.:

sin

2t
1 t 2

The tangent half-angle substitution can also be derived using a graphical method as in the figure
below.

Alternate Planar 3R IPK solution method

The equation form

E cos F sin G 0

arises often in the position solutions for mechanisms and robots. It appeared in the 1 solution of the
Inverse Pose Kinematics solution for the planar 3R robot in the ME 4290/5290 NotesBook and was
solved using the tangent half-angle substitution.
Next we present an alternative and simpler solution to this equation. We make two simple
trigonometric substitutions based on the figure below.

Clearly from this figure we have:

cos

sin

E2 F 2

In the original equation we divide by

E
E F
2

F
E2 F 2

E 2 F 2 and rearrange:

cos

F
E F
2

sin

G
E2 F 2

The two simple trigonometric substitutions yield:

cos cos sin sin

G
E2 F 2

Applying the sum-of-angles formula cos(a b) cos a cos b sin a sin b yields:

cos( )

G
E2 F 2

And so the solution for is:

2
2
E F

1,2 cos1

where

tan 1
E
and the quadrant-specific inverse tangent function atan2 must be used in the above expression for .
There are two solutions for , indicated by the subscripts 1,2, since the inverse cosine function is
double-valued. Both solutions are correct. We expected these two solutions from the tangent-half-angle
substitution approach. They correspond to the elbow-up and elbow-down solutions (the engineer must
determine which is which) for the planar 3R robot IPK solution.
For real solutions for to exist, we must have

G
E F
2

or

G
E F2
2

If this condition is violated for the planar 3R robot, this means that the given input pose x3, y3, is
beyond the robot workspace limits.

Second alternate solution method

For the planar 3R robot IPK problem we now present a second alternate solution method that
does not initially result in the equation form E cos F sin G 0 . We still square and add the XY
position equations, but without isolating the 2 terms first. Here are the original XY equations:

x3 L1 cos1 L2 cos(1 2 )
y3 L1 sin 1 L2 sin(1 2 )
Squaring and adding these equations as-is yields:
x32 L12 cos 2 1 2 L1 L2 cos 1 cos(1 2 ) L22 cos 2 (1 2 )
y32 L12 sin 2 1 2 L1 L2 sin 1 sin(1 2 ) L22 sin 2 (1 2 )
x32 y32 L12 L22 2 L1 L2 cos 1 cos(1 2 ) sin 1 sin(1 2 )

Applying the sum-of-angles formula cos(a b) cos a cos b sin a sin b yields:

x32 y32 L12 L22 2 L1L2 cos(1 (1 2 ))

L12 L22 2 L1L2 cos(2 )
L12 L22 2 L1L2 cos 2
Where we have also used the trigonometric identity cos(a) cos a .
We must deal with trigonometric uncertainty (double-valued inverse trigonometric functions) by
using cos and sin together, rather than just cos. The above equation yields:

cos2

x32 y32 L12 L22

2L1L2

From the trigonometric identity cos 2 2 sin 2 2 1 we obtain:

sin 2 1 cos 2 2
The solution for 2 is then:

2 atan2( sin 2 , cos 2 )

The expected two solution sets (elbow-up and elbow-down) come from the on the square root
in the sin 2 term.

Knowing 2 in this second alternative IPK solution method, we must return to the original XY
equations and solve for 1.

x3 L1 cos 1 L2 cos(1 2 ) L1 cos1 L2 (cos1 cos 2 sin 1 sin 2 )

y3 L1 sin 1 L2 sin(1 2 ) L1 sin 1 L2 (sin 1 cos2 cos1 sin 2 )
x3 ( L1 L2 cos2 )cos1 ( L2 sin 2 )sin 1
y3 ( L1 L2 cos2 )sin 1 ( L2 sin 2 )cos1
x3 k1 cos1 k2 sin 1
y3 k1 sin 1 k2 cos 1

where

k1 L1 L2 cos 2
k2

L2 sin 2

Now, with 2 known, these two XY equations are not independent. Notice both have the form
E cos 1 F sin 1 G 0 . We know two methods to solve this type of equation (tangent half-angle
substitution, plus the cos and sin simple trigonometric substitution). Either equation can be solved
for 1 and will yield identical results, one unique 1 for each 2 branch (elbow-up and elbow-down).
However, if we continue to use both equations, an interesting geometric interpretation will
appear. Let us make the following polar substitution, very similar to the previous cos and sin
simple trigonometric substitution (see the figure below).

k1 r cos
k2 r sin

r k12 k22
where

tan1 2
k
1

We only use the positive square root term in r and the quadrant-specific inverse tangent function atan2
must be used in the above expression for .

10

Applying this substitution to the XY equations yields:

x3 r cos1 cos r sin 1 sin r cos(1 )

y3 r sin 1 cos r cos1 sin r sin(1 )
We can form a ratio of the Y to the X equation to solve for 1, one for each 2 value.

r sin(1 ) y3

r cos(1 ) x3

1 atan2( y3 , x3 )
The interesting geometric interpretation in the planar 3R IPK solution via this method is shown
in the figure below. The entire k1k2r triangle shown above is rotated with 1 (and leads 1 for the
elbow-down solution). r is the distance from the origin of fixed frame {0} to the origin of moving frame
{3}.

k2

L2
r

Y0

k1

L1

X0

y3

x3

This is why 1 was able to cancel out of the equations and leave only 2 for solving first in this
solution method. The length r from the origin of {0} to the origin of {3} is only a function of the elbow
joint angle 2 and does not depend on 1 or 3.

11

2.7.4 Piepers Solution Method

Decoupled Inverse Pose Kinematics Solution

Pieper proved that if a 6-dof robot has any three consecutive joint axes intersecting, there exists a
closed-form (analytical) solution to the inverse position kinematics. The majority of industrial robots
are in this category.
In particular, many robots have a spherical wrist, i.e. three wrist actuators that rotate about a axes
intersecting in a common point. In this case, the position and orientation sub-problems may be
decoupled. We solve for the first three joints first using the position vector input. Then we solve for the
second three joints next using the given orientation, based on the orientation caused by the first three
joints. The given position vector must point to the wrist point (the shared origin point of three
consecutive wrist frames). We can always transform any end-effector or other tool vector to this origin
point, since there are no more active joints beyond the last wrist joint.
PUMA Example (without details):

Given 06T , calculate (1 , 2 , 3 , 4 , 5 , 6 ) .

06T f (1 , 2 , 3 , 4 , 5 , 6 )

60 R (1 , 2 , 3 , 4 , 5 , 6 )

0
6T

0
0
0

P6 (1 , 2 , 3 )

Joints 4, 5, and 6 cannot affect the translation of the wrist origin point.
1) Translational equations: Given

P , calculate ( , , ) values (4 sets).

0

3 independent equations, 3 unknowns.

2) Rotational equations: Given 60 R , and knowing (1 ,2 ,3 ) , calculate (4 ,5 ,6 ) values (2 sets).
3 independent equations, 6 dependent equations, 3 unknowns.
T

63 R( 4 ,5 , 6 ) 30 R (1 , 2 ,3 ) 60 R

4 sets of (1 ,2 ,3 ) ; 2 sets of (4 ,5 ,6 ) for each. Therefore, there are 8 overall solutions to the
inverse position problem for the PUMA. Some solution sets may lie outside joint ranges. Generally one
would choose the closest solution to the previous position.

12

We can use homogeneous transformation equations to isolate and solve for the various
unknowns in turn. I call this peeling-off homogeneous transformations matrices with unknowns to
separate variables. The approach is to multiply by the inverse component homogeneous transformation
matrices as a function of one unknown joint variable each. Below the details are not shown so you must
try this for yourself. The original FPK transform equation is the starting point for IPK, as always.
06T 01T (1 ) 21T ( 2 ) 23T ( 3 ) 34T ( 4 ) 45T ( 5 ) 56T ( 6 )

The left-hand-side 06T is a set of valid given numbers, the input to the IPK problem. Inverting
the first matrix on the left yields:
1

01T (1 ) 06T 21T ( 2 ) 23T (3 ) 34T ( 4 ) 45T (5 ) 56T ( 6 )

from which we can solve for 1 and 3. Now we invert the homogeneous transformation matrix that
combines the first three matrices in the original equation.
1

03T ( 2 ) 06T 34T ( 4 ) 45T (5 ) 56T ( 6 )

We can solve for 2 with 1 and 3 already known. This completes the solution for the arm angle joints
(1 ,2 ,3 ) , possible because the position vector 0 P6 is only a function of (1 ,2 ,3 ) , as stated earlier.
Now we must solve for the wrist joint angles (4 ,5 ,6 ) knowing (1 ,2 ,3 ) . Performing one
more matrix inversion on the left will separate these unknowns sufficiently.
1

04T ( 4 ) 06T 45T (5 ) 56T ( 6 )

No we can isolate and solve for 4, 5, and 6 in turn.

13

2.7.5 Spatial 8R NASA AAI ARMII Robot IPK Solution

Inverse Pose Kinematics General Statement

Given:

Calculate:

Since m = 6 (Cartesian dof) and n = 8 (joint dof) we have the underconstrained kinematicallyredundant case. There are infinite solutions (multiple as well). There are some great ways to make use
of this redundancy for performance optimization in addition to following commanded Cartesian
translational and rotational velocity trajectories. For inverse pose purposes we will here simplify instead
and lock out the redundancy so that m = n = 6; let us choose 3 5 0 for all motion to accomplish
this. Then we have a determined Inverse Pose Kinematics problem with finite solutions, still with
multiple joint angle solution sets.

14

The Forward Pose Kinematics relationship is:

So, the first step should be to simplify the equations as much as possible by calculating the required
08T to achieve the commanded HBT .

The problem can be decoupled between the arm joints 1-4 and the wrist joints 5-8 since the ARMII has a
spherical wrist (all 4 wrist joint Cartesian coordinate frames share the same origin). See the previous
section that explained the Pieper results for the 6-axis PUMA robot.
Now, we will further simplify by ignoring the wrist joints 6-8 (5 is already locked to zero) and solve the
Inverse Pose Kinematics problem only for the arm joints 1,2, and 4. The full inverse solution is given in
Williams 1.
0F

Inverse Pose Kinematics Symbolic Solution for Arm Joints Only, with 3 0
Reduced problem statement
Given B P5 x5 y5

z5 , calculate (1 , 2 , 4 )i , for all possible solution sets i.

T

That is, with only three active joints, we can only specify three Cartesian objectives, in this case the XYZ
location of the origin of {5} with respect to origin of {B} (and expressed in the basis of {B}). Note that
B P5 B P8 due to the spherical wrist.
The equations to solve for this problem come from the Forward Pose Kinematics relationships
for the ARMII robot, the translational portion only (further, with 3 0 ).

This equation yields (the derivation is left to student, use symbolic Forward Pose Kinematics):

x5 c1 (d3 s2 d5 s24 )

P5 y5 s1 (d3 s2 d5 s24 )
z d d c d c
3 2
5 24
5 B

where

c24 cos( 2 4 )
s24 sin( 2 4 )

(Since 3 0 always, the Z axes of 2 and 4 are always parallel and we used the sum-of-angles trig
formulas.)

1
R.L. Williams II, Kinematic Equations for Control of the Redundant Eight-Degree-of-Freedom Advanced Research Manipulator II,
NASA Technical Memorandum 4377, NASA Langley Research Center, Hampton, VA, July 1992.

15

Solution Process

1.

A ratio of the Y to X equations yields:

1 180 is also a valid solution

2.

Since 1 is now known (two values), we can modify the Y and Z equations.

y5
s1

where:

Z z5 d B
Isolate the ( 2 4 ) terms:

Square and add to eliminate 4:

The result is one equation in one unknown 2.

E cos 2 F sin 2 G 0
E 2Zd3

where

F 2Yd3
G Y 2 Z 2 d32 d52

We can solve this equation for 2 by using the Tangent Half-Angle Substitution. We presented this
back in the Inverse Pose Solution of the planar 3R robot; we solve for 2 (in that section, it was for 1).
Solve for 2 by inverting the original Tangent Half-Angle definition.

16

Two 2 solutions result from the in the quadratic formula; both are correct (there are multiple
solutions elbow up and elbow down). To find 4, return to original (arranged) translational equations.

d5 s24 Y d3s2
d5c24 Z d3c2
Find the unique 4 for each 2 value (use the quadrant-specific atan2 function in MATLAB).

Solutions Summary

The solution is now complete for the ARMII robot reduced inverse pose problem (translational
joints only, plus 3 0 ).
There are multiple solutions since there are two values for 1. For each 1, there are two values
for 2; for each valid (1 , 2 ) , there is a unique 4. So there are a total of four (1 , 2 , 4 ) solution sets
for this reduced problem. We can show this with the PUMA model (its not the same robot, but it has
similar joints when 3 0 ).
These four solution sets occur in a very special arrangement pattern, summarized in the table
below.
i

1 180

2 2

1 180

21

In all numerical examples, you can check the results; plug all solution sets (1 , 2 , 4 ) one at a
time into the Forward Pose solution and verify that all sets yield the same, commanded

P .
B

You can

also calculate the associated 4B R . All of these resulting rotation matrices should be different (why?).

17

8R ARMII Robot Translational Inverse Pose Kinematics Example

Given

P , calculate ( , , )
B

4 i

i 1, 2,3, 4 .

x5 0.6572

P5 y5 0.1159
z 1.6952
5

Answers (deg)

10

20

30

10

46.6

-30

190

-46.6

30

190

-20

-30

Check all solution sets via Forward Pose Kinematics to ensure all yield the correct, commanded

P .
B

18

2.8 Trajectory Generation

2.8.4 Single Fourth-Order Polynomial with a Via Point

We can achieve the same goals as the two third-order polynomials meeting at a via point much
more simply. We use a single polynomial, forced to go through a via point. Here are the constraints for
meeting the required angles with smooth motion (since we use a single continuous polynomial, the
velocity and accelerations are guaranteed to match at the via point). This is original work presented at
the 2011 ASME IDETC in Washington DC2.

Note in this case since there is only one time range, it is convenient to treat all times as absolute, rather
than relative as when we did matching of two third-order polynomials. With five constraints we can use
a single fourth-order polynomial.

(t ) a4t 4 a3t 3 a2t 2 a1t a0

(t ) 4a4t 3 3a3t 2 2a2t a1
(t ) 12a4t 2 6a3t 2a2
(t ) 24a4t 6a3

Similar to previous derivations, from two constraints we immediately have:

a0 S

a1 0

The remaining coupled unknown polynomial coefficients linear equations may be expressed by the
following 3x3 matrix/vector equation.

tV2
2
tF
2tF

tV3
tF3
3tF2

a2 V S

a3 F S
4tF3 a4 0
tV4
tF4

1 tV

1 t F

2 3tF

a2 (V S ) tV2

2
a3 ( F S ) tF

4tF2 a4
0

tV2
tF2

R.L. Williams II, 2011, Improved Robotics Joint-Space Trajectory Generation with Via Point, Proceedings of the ASME
IDETC/CIE, Washington DC, August 28-31, 2011, DETC2011-47592.

19

The 3x3 coefficient matrix determinant is tV2 t F4 (t F tV ) 2 ( t F (t F tV ) 2 ), so as long as neither tV 0 nor

tF 0 , this solution always exists. t F tV is required in this scenario, so that term cannot be zero.
Single Fourth-Order Polynomial with Via Point Analytical Solution

a0 S
a1 0
(V S )tF2 ( F S )(4t F 3tV )tV
1

tV2
tF2
(tF tV ) 2

2
2
(V S )tF ( F S )(2t F tV )
2
a3

2
tV2
tF3
(tF tV )

a2

a4

1
(tF tV ) 2

(V S ) ( F S )(3t F 2tV )

2
t F3
tV

For the special case of the via time being half the final time ( tV t F 2 ), the analytical solution is:

a0 S
a1 0
a2

1
16(V S ) 5( F S )
t F2

a3

2
16(V S ) 7( F S )
t F3

a4

8
2(V S ) ( F S )
t F4

20

Example Single Fourth-Order Polynomial with Via Point

Use a single fourth-order polynomial for smooth joint space trajectory generation, plus motion
through an additional via point (no need to stop at the via point, only ensure smooth motion), for one
joint only.

S 3 0 , V 1 8 0 , F 1 2 0 , tV 1.5, tF 3 sec
(t) for smooth trajectory generation and continuous motion through a via point. Plot
(t ),(t ),(t ),(t ) .

Given:
Find:

(t ) 20.74t 4 131.11t 3 216.67t 2 30

Result

(t ) 82.96t 3 393.33t 2 433.33t

(t ) 248.89t 2 786.67t 433.33

(Note deg units are used throughout)

(t ) 497.78t 786.67

150
100
50
0

0.5

1.5

2.5

0.5

1.5

2.5

0.5

1.5

2.5

0.5

1.5
time (sec)

2.5

100
50
0
-50
0

dd

400
200
0
0

ddd

1000

-1000
0

The (t) shape for the single fourth-order polynomial is very similar to that for the two third-order
polynomials. Again the joint angle passes through the via point V 180 and keeps going briefly, due

21

to the fact that the velocity is still positive beyond the first 1.5 sec. The peak for this fourth-order case is
slightly greater and occurs in time slightly after the peak for the two third-order polynomials case. The
maximum angle is M A X 1 85 .4 , occurring at t = 1.74 sec. For the fourth-order polynomial, a side
benefit has arisen. The jerk is now continuous at the via time, where it was discontinuous for the two
matched third-order polynomials.
The jerk still has an infinite spike at the start and end as we saw with both previous third-order
polynomial examples, which is unacceptable. We correct this with a single sixth-order polynomial as
presented in Section 2.8.5 in the ME 4290/5290 NotesBook. Again, that work is original.

22

2.9 Velocity Kinematics

2.9.3 Jacobian Matrices
Four ways to derive the Jacobian matrix
1) Time derivatives and relative angular velocity equation

This subsection is presented in examples in the ME 4290/5290 NotesBook.

2) Jacobian matrix column as the end-effector translational and rotational velocity due to joint i

This subsection is also presented in the ME 4290/5290 NotesBook.

3) Partial derivatives definition closely related to the second method

f vector of m Cartesian functions. This relationship holds only for translational part. That is, no
Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.
4) Velocity recursion a'la Craig 3
Add translational and rotational velocities serially link-by-link from the base to the end-effector link.
1F

Revolute joint

i 1 i i1R ii

i 1

Prismatic joint

i 1

vi 1 i i1R

i 1

Z i 1 i 1

v P
i

i 1

i 1 i i1R ii

i 1

Start with

i 1

vi 1 i i1R

v 0
0

v P
i

i 1

i 1

Z i 1 di 1

0 0 (if the robot base frame {0} is fixed).

T

Use the velocity recursion equations, i 0,1, 2,, N 1 . Then, factor out
to get the Jacobian matrix

from

v ,
N

J .

J.J. Craig, 2005, Introduction to Robotics: Mechanics and Control, Third edition, Pearson Prentice Hall, Upper Saddle River, NJ.

23

3) Derive

J via

Partial derivatives definition

Example for the planar 3R robot.

0

J
f

px

P p
0

pi 3 pi d j 3 pi

j
t j 1 j dt
j 1 j

i = x, y

px

px

px

py

py

py

The angular velocity vector is found from the relative angular velocity equation.
0

0
1

c1
s1
0

s1
c1
0

0
2

0 0 c12

0 0 s12
1 1 0

s12
c12
0

0
3

The overall Jacobian matrix in {0} coordinates is:

0
H

0 0 c123

0 0 s123
1 2 0

0
0
0
1 2 3 Hz

s123
c123
0

0 0 c123

0 0 s123
1 3 0

s123
c123
0

0 0

0 0
1 0

24

2.10 Acceleration Kinematics

The acceleration vector is the first time derivative of the velocity vector (and the second time
derivative of the position vector). It is linear in acceleration terms but nonlinear in rate components.
Both Cartesian acceleration and velocity are vectors, with translational and rotational parts.
Acceleration Kinematics Analysis is useful for
Resolved Acceleration Control
Acceleration of any point on manipulator
Moving objects in workspace smooth trajectory generation
Required for Dynamics
Translational Acceleration
k

A is the translational acceleration of the origin of frame {j} with respect to reference frame
i

{i}, expressed in the basis (coordinates) of {k}.

Transport Theorem (Craig, 2005)
d A
B R
dt

Q
B

A
B

R BVQ AB BA R BQ

General five-part acceleration equation

Position
A P A PB BA R B P

P
A

T B P

A
B

Velocity
A
V A PB BA R B P BA R
V V0 VP P

P
B

Acceleration
A A dtd AVB BA R BVP AB BA R B PP

A A
A

A
B

R B AP 2 A B BA R BVP A B BA R B PP A B

A A0 AP 2 V P P

A
B

R B PP

25

Rotational Acceleration
k

is the rotational acceleration of frame {j} with respect to reference frame {i}, expressed
i

in the basis (coordinates) of {k}. No angular orientation exists for which we can take the time derivative
to find the angular velocity. Instead we use relative velocity equations.
Rotational Velocity
A
C AB BAR

Rotational Acceleration
A C dtd AB BA R BC A B dtd BA R BC
A C A B BA R B C AB BA R BC

For vectors expressed in the local frame

A

A
B

A
C

A
B

A
C

Combined Translational and Rotational Acceleration

a
X

where

A

a

both a and are (3x1) vectors

Acceleration Example

Planar 2R robot acceleration of end-effector frame with respect to {0}, also expressed in {0}.
1) Craig (2005) acceleration recursion

L1

P2 0
0

c2
R s2

L2

P3 0
0

2
1

0
0
1

s2
c2
0

0
00 0
0

0
00 0
0

0
0a0 0
0

0
1 0

1

0
1 0

1

0

a1 0
0

23 R I 3

26

a
2

0
2
2 0

1 2

0
2
2 0

2
1

P P a
1

3
2

0
3

R 3 a3

c2
0

P P a
2

Rewrite for comparison to results in the next section.

L1s1 L2 s12

1 1

2 12

0 L112 L1c212 L1s21

0 L11 L1s212 L1c21

1 0
0

s2

L1 ( s11 c112 ) L2 s12 (1 2 ) c12 (1 2 ) 2

0
a3 L1 (c11 s112 ) L2 c12 (1 2 ) s12 (1 2 )2

a Lc L c

L2 (1 2 )2 L1c212 L1s21
3a3 L2 (1 2 ) L1s212 L1c21

a
0

c2
2 a2 s2
0

0
3
2
3 2 0

1 2

2
1

L2 s12 1 L1c11

L2 c12 2 L1s11

The second term above can be written as

L1c11 L2 c12 (1 2 ) L2 c12 (1 2 ) 1

L1s11 L2 s12 (1 2 ) L2 s12 (1 2 ) 2

L2 c12 (1 2 ) 1

L2 s12 (1 2 ) (1 2 )

27

Acceleration Example (cont.)

Planar 2R robot acceleration of the end-effector frame with respect to {0}, also expressed in
{0}. An alternative method is presented here.
2) Differentiation of rate equation

X J

X J J

Do in frame {0} if in frame {2} or {3}, one must account for r .

L1s1 L2 s12
J L1c1 L2 c12

1
0

L2 s12
L2 c12
1

d J dJ ij
J

dt
dt

dJij
dt

Jij d1 Jij d2
J d

ij N
1 dt 2 dt
N dt
( L1c1 L2c12 )1 L2c122

0 J ( L1s1 L2 s12 )1 L2 s122

L1c11 L2 c12 (1 2 )

0 J L1 s11 L2 s12 (1 2 )

X
0

L1 s1 L2 s12
0
X L1c1 L2c12

dJij
dt

Jij
k
k 1 k
N

L2c121 L2c122

L2 s121 L2 s122

L2 c12 (1 2 )

L2 s12 (1 2 )

J 0 J

L1c11 L2 c12 (1 2 )
L2 s12

L2 c12 1 L1 s11 L2 s12 (1 2 )

1 2
0

L2 c12 (1 2 )

L2 s12 (1 2 ) 1

2
0

This yields the same result as before.

28

Uses for general acceleration equation

X J
X J J
J X J
1

i. Forward Acceleration Kinematics Analysis X J J predicts the Cartesian

accelerations X given the joint rates and accelerations.

ii. Resolved Acceleration Control like resolved rate, but acceleration is commanded instead of
1
velocity. Solve J X J and double integrate to get the commanded joint

angles.
iii. Dynamics equations is required for the Newton/Euler dynamics recursion in this ME
4290/5290 Supplement, Chapter 5. If acceleration is calculated via numerical differentiation,
1
numerical instability can result, so the analytical approach J X J is better.

Now, if inverse dynamics control is being used in the resolved-rate control algorithm
framework, assume X is constant and so X 0 . In this case:

How can we find the time rate of change of the Jacobian matrix J ? See the previous page for a
specific example.

29

4. Parallel Robots
4.3 Planar 3-RPR Manipulator
4.3.1 Planar 3-RPR Manipulator Inverse Pose Kinematics

Planar 3-RPR Manipulator Kinematic Diagram

Inverse Pose Kinematics (for pose control)
x, y,
Given:
L1 , L2 , L3
Find:

Vector loop-closure equations:

P
0

0
H

PCi 0 A i 0 Li

i 1, 2,3

The vector loop-closure equations are rewritten below:

L P
0

0
H

PCi 0 A i

i 1, 2,3

The inverse pose solution is straight-forward, found independently for each of the three legs. The
Euclidean norm is used in the equations below.

Li

L P
0

0
H

PCi 0 A i

i 1, 2,3

30

We can also calculate the intermediate passive joint variables 1,2 ,3 , (independently for each of the
three legs) for use in velocity and dynamics analyses. The quadrant-specific atan2 function must be
used in the equations below.

Ciy Aiy

Cix Aix

i tan 1

i 1, 2,3

31

4.3.2 Planar 3-RPR Manipulator Forward Pose Kinematics

3-RPR parallel robot Forward Pose Kinematics (for simulation and sensor-based control)
Given:

L1 , L2 , L3

Find:

x, y,

This is a coupled, nonlinear problem to solve it is difficult to solve and multiple solutions generally
exist, like the Inverse Pose Kinematics problem for serial robots. We use the same vector loop-closure
equations from Inverse Pose Kinematics, repeated and with details filled in below:

L P
0

0
H

PCi 0 A i

i 1, 2,3
Li c i x c

Li si y s

s H Cix 0 Aix

c H Ciy 0 Aiy

Considering all three legs simultaneously (the problem is coupled and nonlinear), these represent six
scalar equations in the six unknowns x, y , , 1 , 2 , 3 .
We can use the Newton-Raphson numerical iteration technique to solve this Forward Pose
Kinematics Problem. We can directly solve the above six equations for the six unknowns.

Li ci x c H Cix s H Ciy 0 Aix

Li si y s H Cix c H Ciy 0 Aiy

i 1, 2,3

However, we dont always need the intermediate variables 1 ,2 ,3 (also, we can calculate these
angles later, using Inverse Pose Kinematics, if required for velocity, dynamics, or computer simulation).
So, to simplify the Forward Pose Kinematics Problem, square and add each XY equation pair (for all
three legs) to eliminate the intermediate variables 1 , 2 ,3 . Then we will have three equations to solve
for the three primary unknowns x , y , . This problem is solved via the Newton-Raphson iterative
numerical method in Section 4.4.

x 2 y 2 Aix2 Aiy2 Cix2 Ciy2 L2i 2( xAix yAiy ) 2c ( xCix yCiy AixCix AiyCiy )
2 s ( xCiy yCix AixCiy AiyCix ) 0
where known constants 0 Aix , 0 Aiy , H Cix , H Ciy were shortened to Aix , Aiy , Cix , Ciy for clarity.

i 1, 2,3

32

Alternate analytical 3-RPR manipulator forward pose solution

This 3-RPR robot forward pose problem is equivalent to finding the assembly configurations of
a four-bar mechanism with known input/output link lengths L1, L2 and an RR constraining dyad of
known length L3. By itself the four-bar mechanism has infinite assembly configurations because it has
one-dof. RR dyad A3 C 3 constrains the mechanism to a statically-determinate structure of 0-dof. Point
C 3 defines a four-bar coupler curve which is a tricircular sextic (sixth-degree algebraic curve) that has a
maximum of six intersections with the circle of radius L3 centered at A3 (Hunt, 1990).

Figure:

Branch A1C1C2 A2 is a 4-bar mechanism with input angle 1 and output angle 2 (both
unknowns). With given lengths L1 and L2, this four-bar mechanism has 1-dof, and it can trace out a
coupler curve for point C3 in the plane. In general, this coupler curve is a tricircular sextic. The
forward pose kinematics solution may be found by intersecting leg A3C3 (a circle of given radius L3,
centered at known centerpoint A3) with the coupler curve. There are at most six intersections between a
circle and tricircular sextic and so there may be up to six real multiple solutions to the 3-RPR parallel
robot forward pose kinematics problem. There are always six solutions, but 0, 2, 4, or 6 of these will be
real, depending on the commanded configuration and robot geometry.

33

4.3.3 Planar 3-RPR Manipulator Velocity Kinematics

First the pose configuration variables must all be known. Then we can define and solve two problems.
Forward velocity kinematics (for simulation)
Given:

L1 , L2 , L3

Find:

x , y , z

where z

Inverse velocity kinematics (for resolved-rate control)

Given:

x , y , z

Find:

L1 , L2 , L3

In both cases intermediate passive joint rate unknowns 1 , 2 , 3 are involved. Both velocity
kinematics problems use the same rate equations; we will derive these from looking at the three single
RPR legs separately (meeting at the end-effector). Here is the figure for the ith leg:

Planar 3-RPR Manipulator Velocity Diagram, leg i

As usual, the velocity equations will be obtained by a time derivative of the applicable pose equations.
The vector loop-closure equation for leg i is:

P A L
0

L Hi

i 1, 2,3

34

The XY component equations are:

x 0 Aix Li c i LHi cos( i i )

i 1, 2,3

y 0 Aiy Li s i LHi sin( i i )

and the angle equation is:

i i i

i 1, 2,3

i for use in velocity equations:

1 1 1
2 2 2 120
3 3 270
These i relationships assume symmetry, with an equilateral end-effector triangle having
1 2 3 30 .
The velocity equations for one RPR leg are obtained from the first time derivatives of the XY
and angle equations.

x Li s ii Li c i LHi sin( i i )(i i )

y L c L s L cos( )( )
i

i i

Hi

i 1, 2,3

i i
These equations are written in matrix-vector form to yield the RPR leg Jacobian matrix.

x Li s i LHi sin( i i ) c i

y Li c i LHi cos( i i ) s i

1
0
z

LHi sin( i i ) i

LHi cos( i i ) Li

1
i

Written in compact notation:

X J
i

i 1, 2,3

i 1, 2,3

35

X x

y z is the same for all three legs (the Cartesian end-effector rates).
T

includes one

active and two passive joint rates for each of the three RPR legs, i 1, 2,3 . Let us now find the overall
robot Jacobian matrix, using only active rates and ignoring the passive rates. Invert the leg Jacobian
matrix symbolically:

s i

i Li

Li c i

i s i
Li

c i
Li
s i
c i
Li

LHi c i
x
Li

LHi s i y

L c
1 Hi i z
Li

i 1, 2,3

Clearly the only singularity condition for this operation is when Li 0 , which is generally impossible
by design for the 3-RPR parallel robot.
Extract only the active joint row of this result and assemble all three active joint rows into the
overall robot Jacobian matrix:

L1 c1

L2 c 2
L c
3 3

s1
s 2
s 3

LH 1 s1 x

LH 2 s 2 y
LH 3 s 3 z

L MX
This above equation solves the Inverse Velocity Problem. No inversion is required and so the Inverse
Velocity problem is never singular. Above it is expressed in compact notation. Note inverse velocity
Jacobian matrix [M] is closely related to the Newton-Raphson Jacobian matrix [JNR] from the numerical
FPK solution.
Forward Velocity Problem

This problem is obtained by inverting the Inverse Velocity Solution:

X M L
1

Ironically, it is the Forward Velocity Problem that is subject to singularities for parallel robots.

36

Example Symbolic MATLAB Code

Here is the MATLAB code that was used to symbolically invert the RPR leg Jacobian matrix as
presented above. Symbolic computing has a lot of power in robot kinematics, dynamics, and control.

%
%
%

Symbolic MATLAB code to invert the RPR leg Jacobian

clear; clc;
%
L
LH
t1
b1
c1
s1
cp
sp

=
=
=
=
=
=
=
=

Declare symbolic variables

sym('L');
sym('LH');
sym('t1');
sym('b1');
sym('cos(t1)');
sym('sin(t1)');
sym('cos(t1+b1)');
sym('sin(t1+b1)');

%
j11
j21
j13
j23

Jacobian elements
= -L*s1-LH*sp;
= L*c1+LH*cp;
= -LH*sp;
= LH*cp;

%
Jacobian matrix
J = sym([j11 c1 j13;j21 s1 j23;1 0 1]);
%
Invert
Jinv = inv(J);
%
Simplify
Jinvsimp = simple(Jinv);
%
Check
Ident1 = simple(Jinvsimp * J);
Ident2 = simple(J * Jinvsimp);

Note: the first four lines of the declaration statements of the m-code above may be replaced succinctly
with:
syms L LH t1 b1;

37

4.4 Newton-Raphson Method

The Newton-Raphson Method involves numerical iteration to solve coupled sets of n nonlinear
equations (algebraic/transcendental not ODEs) in n unknowns. It requires a good initial guess of the
solution to get started and it only yields one of the possible multiple solutions. The Newton-Raphson
method is an extension of Newtons single function/single variable root-finding technique to n functions
and n variables. The following is the form of the given functions to solve.

F(X) 0
where the n functions are

F(X) F1 (X)

and the n variables are

X x1

F2 (X) Fn (X)

x2 xn

Perform a Taylor Series Expansion of {F} about {X}:

Fi
x j O X2

x
j 1
j

Fi X X Fi X

i 1, 2, , n

F
Where J NR J NR ( X) i is the Newton-Raphson Jacobian Matrix, a multi-dimensional form of
x j

the derivative and a function of {X}. If X is small, the higher-order terms O X 2 from the
expansion are negligible. For solution, we require:

Fi X X 0

i 1, 2, , n

Now with O X 2 0 we have:

Fi
x j Fi X J NR X 0
j 1 x j
n

Fi X X Fi X

i 1, 2, , n

So to calculate the required correction factor X at each solution iteration step, we must solve
F X J NR X 0 .

X J NR

F X

Solution via Gaussian elimination on J NR X F X is preferable numerically, since this is

more efficient and more robust than matrix inversion.

38

Newton-Raphson Method Algorithm Summary

0)

Establish the functions and variables to solve for: F X 0

1)

Make an initial guess to the solution: X0

2)

Solve J NR Xk Xk F Xk for X k , where k is the iteration counter.

3)

Update the current best guess for the solution: X k 1 X k X k

4)

Iterate until

Xk , where we use the Euclidean norm and is a small, user-defined

scalar solution tolerance. Also halt the iteration if the number of steps becomes too high
(which means the solution is diverging). Generally less than 10 iterations is required for
even very tight solution tolerances.
If the initial guess to the solution X0 is sufficiently close to an actual solution, the NewtonRaphson technique guarantees quadratic convergence.
Now, for manipulator forward pose problems, the Newton-Raphson technique requires a good
initial guess to ensure convergence and yields only one of the multiple solutions. However, this does
not present any difficulty since the existing known pose configuration makes an excellent initial guess
for the next solution step (if the control rate is high, many cycles per second, the robot cannot move too
far from this known initial guess). Also, except in the case of singularities where the multiple solution
branches converge, the one resulting solution is generally the one you want, closest to the initial guess,
most likely the actual configuration of the real robot.
There is a very interesting and beautiful relationship between numerical pose solution and the
velocity problem for parallel robots. The Newton-Raphson Jacobian Matrix is nearly identical to the
Inverse Velocity Jacobian Matrix for parallel robots. (In the planar case it is identical, in the spatial it is
related very closely.) This reduces computation if you need both forward pose computation and inversevelocity-based resolved-rate control.
3-RPR manipulator forward pose kinematics solution

Use the Newton-Raphson numerical iteration method for solution, with:

X x

The three coupled, nonlinear, transcendental functions Fi are the squared and added equations for
each RPR leg, with L2i brought to the other side to equate the functions to zero.

39

Derive the required Newton-Raphson Jacobian Matrix.

F
J NR X i

x j

Fi
2 x 2(Cix c Ciy s ) 2 Aix
x

i 1, 2,3

Fi
2 y 2(C ix s C iy c ) 2 Aiy
y

i 1, 2,3

Fi
2 s ( xC ix yC iy Aix C ix Aiy C iy ) 2 c ( xC iy yC ix Aix C iy Aiy C ix )

i 1, 2,3

where

Aix

iy

A A
0

and

Cix
Ci
Ciy

Use i 1, 2,3 in the above definitions in the proper places in the overall Newton-Raphson
Jacobian Matrix. After the forward pose problem is solved at each motion step, we can calculate the
intermediate variables 1,2 ,3 as in inverse pose kinematics solution above.

40

Alternate 3-RPR manipulator forward pose Newton-Raphson solution

As we said, we could have solved the original six equations in the six unknowns including the
three intermediate variables 1,2 ,3 . The functions are simpler (no squaring and adding) but the size of
the problem is doubled to i 1, 2, , 6 . Below is the required Jacobian Matrix for this case, where the
odd functions are the X equations and the even functions are the Y equations; also the variable ordering
T
is X x y 1 2 3 .

1
0

1
J NR X

P1x s P1 y c

L1s1

1 P1x c P1 y s
0 P2 x s P2 y c

L1c1
0

0
L2 s 2

1 P2 x c P2 y s
0 P3 x s P3 y c

0
0

L2 c 2
0

P3 x c P3 y s

0
0

0
L3 s3

L3c3
0

41

4.5 Parallel Manipulator Workspace

Since reduced workspace of parallel robots (when compared to serial robots) is their chief
disadvantage, it becomes very important to determine the workspace of parallel robots and maximize it
through design.
There are two workspaces to consider (the same as for serial robots in Section 2.6 of the ME
4290/5290 NotesBook).
1)

reachable workspace is the volume in 3D space reachable by the end-effector in any

orientation

2) dexterous workspace is a subset of the reachable workspace because it is the volume in 3D

space reachable by the end-effector in all orientations.
For parallel robots, the dexterous workspace is almost always null since the rotation capability
is never full for all three Euler angles; therefore we usually define a reduced dexterous workspace
wherein all Euler angles can reach 30 or some other user-definable range.
3-RPR Example
For planar parallel robots we can generally find the reachable workspace using a geometric
method, figuring out what the end-effector can reach guided by each leg on its own, and then
intersecting the results.

Example 3-RPR Reachable Workspace

For determination of the dexterous workspace, it is most convenient to numerically or

geometrically generate in MATLAB the reachable workspace for different values (end-effector
orientation) within the desired limits. Then stack these up and intersect them to find the dexterous
workspace, defined for a reduced desired rotational range LIMIT .

42

4.6 Planar 2-dof Five-Bar Parallel Robot

The Planar 2-dof Five-Bar Parallel Robot is similar to the Planar 1-dof Four-Bar Mechanism.
There is an extra moving link and another revolute joint, providing 2-dof instead of 1-dof. Therefore,
there must be two motors (ground-mounted), providing input angles 2 and 5 independently as shown
in the diagram below.

Planar 2-dof Five-Bar RRRRR Parallel Robot

The RRRRR designation means that there are five revolute joints in this parallel robot, and the
first and last (the two grounded revolute joints) are actuated via independent motors. The three
intermediate moving revolute joints are passive.
The planar five-bar has five total rigid links (four moving and one ground link) and five revolute
joints, each 1-dof joints (J1). Therefore the total number of degrees-of-freedom (dof), or Mobility M, is:

M 3( N 1) 2 J1 1J 2
M 3(5 1) 2(5) 1(0)
M 2 dof
Since M > 1, the planar five-bar qualifies as a robot, and it is a parallel robot since there are two paths
from the ground link to the end-effector.
The end-effector could be located on link 3 or link 4 (see kinematic diagram below). For
generality, we will place the end-effector point to coincide with Point B, the point where links 3 and 4
intersect with a revolute joint.

43

4.6.1 Pose Kinematics

Step 1. Draw the Kinematic Diagram

r1
r2
r3
r4
r5

constant ground link length

constant input link length
constant passive link length
constant passive link length
constant second input link length

1
2
3
4
5

constant ground link angle

variable input angle
variable passive angle
variable passive angle
variable second input angle

Link 1 is the fixed ground link. All angles 1 through 5 are absolute, measured in a right-hand sense
from the horizontal X direction to each link.
4.5.1.1 Forward Pose Kinematics

Step 2. State the Problem

Given

r1, 1, r2, r3, r4, r5; plus 2 independent input angles 2 and 5

Find

end-effector point B Bx

By plus3 and 4
T

44

Step 3. Draw the Vector Diagram. Define all angles in a positive sense, measured with the right hand
from the right horizontal to the link vector (tail-to-head; your right-hand thumb is located at the vector
tail and your right-hand fingers point in the direction of the vector arrow).

Step 4. Derive the Vector-Loop-Closure Equation. Starting at one point, add vectors tail-to-head until
you reach a second point. Write the VLCE by starting and ending at the same points, but choosing a
different path.

r 2 r 3 r1 r 5 r 4
Step 5. Write the XY Components for the Vector-Loop-Closure Equation. Separate the one vector
equation into its two X and Y scalar components.

r2c2 r3c3 rc
1 1 r5c5 r4c4
r2 s2 r3s3 r1s1 r5 s5 r4 s4
where the following abbreviations have been used:

ci cosi
si sin i

i 1, 2,3, 4,5

45

Step 6. Solve for the Unknowns from the XY equations. Step 5 yielded two coupled nonlinear
transcendental equations in the two unknowns 3, 4. Though difficult, we can solve this problem
analytically. It is very similar to the planar 1-dof four-bar mechanism position analysis solution, since it
involves the intersection of two known circles.
Rewrite the above XY equations to isolate the 3 unknown on one side of both equations.

r3c3 rc
1 1 r5c5 r4 c4 r2c2 a r4c4 r2 c2
r3s3 r1s1 r5 s5 r4 s4 r2 s2 b r4 s4 r2 s2
where:

a rc
1 1 r5c5
b r1s1 r5s5

Then square and add the two XY pose kinematics equations and eliminate 3 using the trig identity
cos 2 3 sin 2 3 1 . This results in a single nonlinear transcendental equation in the single unknown 4:

E cos 4 F sin 4 G 0
where:

E 2r4 (a r2c2 )
F 2r4 (b r2 s2 )
G a 2 b2 r22 r32 r42 2r2 (ac2 bs2 )

Or, unsubstituting terms a and b and simplifying:

E 2r4 (r1c1 r5c5 r2 c2 )
F 2r4 (r1s1 r5 s5 r2 s2 )
G r12 r22 r32 r42 r52 2r1r5 cos(5 1 ) 2r1r2 cos( 2 1 ) 2r2 r5 cos(5 2 )

We can solve this equation for 4 by using the Tangent Half-Angle Substitution (derived in this ME
4290/5290 Supplement).
If we define

t tan

4
2

then

1 t2
cos 4
1 t2

Substitute the Tangent Half-Angle Substitution into the EFG equation:

1 t2
2t
E
F
G 0
2
2
1 t
1 t

E (1 t 2 ) F (2t ) G (1 t 2 ) 0

and

sin 4

2t
1 t 2

46

(G E )t 2 (2 F )t (G E ) 0
This equation is easily solved using the quadratic formula:
t1,2

F E2 F 2 G2
GE

Solve for 4 by inverting the original Tangent Half-Angle Substitution definition:

4 2 tan 1 (t1,2 )
1,2

The atan2 function need not be used in the above equation for 4, due to the multiple 2.
Two 4 solutions result, from the in the quadratic formula. Both are correct since there are two valid
solution branches end-effector up and end-effector down. To find 3, return to the translational pose
kinematics equations, with 3 terms isolated.

3 atan2(r1s1 r5 s5 r4 s4 r2 s2 , r1c1 r5c5 r4c4 r2c2 )

1,2

1,2

1,2

This equation calculates the unique 31,2 for each 41,2 value. In this case we must use the
quadrant-specific atan2 function in MATLAB.
Thus, there are two solution sets for the forward pose kinematics problem for the planar RRRRR
five-bar robot. Be sure to keep the solutions together, i.e. ( 3 , 4 )1 and ( 3 , 4 ) 2 .
The ultimate forward pose kinematics solution, point B, may now be found since the passive
angles for links 3 and 4 are now known. There will be two possible point B branch values, again
corresponding to end-effector up and end-effector down. Both equations below will yield the same
results for the point B values.

B r2 r3
r2c2 r3c31,2
Bx

By 1,2 r2 s2 r3 s31,2

B r1 r 5 r 4
rc
Bx
1 1 r5c5 r4c41,2

By 1,2 r1s1 r5 s5 r4 s41,2

47

4.6.1.2 Inverse Pose Kinematics

Given

r1, 1, r2, r3, r4, r5; plus desired end-effector point B Bx

Find

actuator angles 2 and 5, plus passive angles 3 and 4

By

The actuator unknowns 2 and 5 can be found independently from the two vector loop-closure
equations presented at the end of the Forward Pose Kinematics section. Passive unknown angle 3 can
be found along with 2, and passive unknown angle 4 can be found along with 5.

B r2 r3

B r1 r 5 r 4

Bx r2c2 r3c3

By r2 s2 r3 s3

Bx r1c1 r5c5 r4c4

By r1s1 r5 s5 r4 s4

Isolate the passive unknown angle in each of the above XY scalar equations:
r3c3 Bx r2 c2

r4 c4 Bx r1c1 r5c5

r3 s3 By r2 s2

r4 s4 By r1s1 r5 s5

Squaring and adding both sets separately will eliminate the passive unknowns for now:

E2 cos2 F2 sin 2 G2 0

E5 cos5 F5 sin 5 G5 0

E2 2r2 Bx

E5 2r5 (Bx rc
1 1)

F2 2r2 By

F5 2r5 (By r1s1 )

G2 r22 r32 Bx2 By2

G5 r12 r52 r42 Bx2 By2 2r1 ( Bxc1 By s1 )

Solving independently for the two actuator unknowns using the tangent half-angle substitution twice:
t 21,2

F2 E 22 F22 G22
G2 E 2

2 2 tan 1 (t 2 )
1,2

1,2

t51,2

F5 E52 F52 G52

G5 E5

5 2 tan 1 (t5 )
1,2

1,2

Now return to the original XY scalar equations to solve for the passive unknowns:

3 atan2( By r2 s2 , Bx r2c2 )
1,2

4 atan2( By r1s1 r5 s5 , Bx rc
1 1 r5c5 )
1,2

48

There are two possible values (left and right branches) for unknowns 2 and 3; independently
there are two possible values (left and right branches) for unknowns 5 and 4. Therefore, there are four
overall valid solutions to the Inverse Pose Kinematics problem for the planar parallel five-bar robot.

4.6.2 Velocity Kinematics

Step 1. The five-bar robot Position Analysis must first be complete.

Step 2. Identify the five-bar robot velocity parameters.

i i (i = 2,3,4,5) is the absolute angular velocity of link i. 1 1 0 since link 1, the fixed
ground link, cannot rotate.
The vector loop-closure equations developed in the Pose Kinematics section are the starting
point for deriving the velocity equations.

4.6.2.1 Forward Velocity Kinematics

Step 3. State the Problem

Given

r1, 1, r2, r3, r4, r5; angles 2, 3, 4, and 5; plus 2 and 5

Find

end-effector translational velocity B x

y plus 3 and 4
T

Step 4. Derive the velocity equations. Take the first time derivative of the vector loop closure
equations from position analysis, in XY component form, repeated below.

r2c2 r3c3 rc
1 1 r5c5 r4c4
r2 s2 r3s3 r1s1 r5s5 r4 s4
The first time derivative of the position equations is given below:

r22 s2 r33s3 r55s5 r44 s4

r c r c r c r c
2 2 2

3 3 3

5 5 5

4 4 4

Since all links are rigid (i.e. no links are changing lengths), all ri 0 and thus the four XY pairs
of terms above represent the absolute tangential velocities of the endpoint of each link.
Gathering unknowns on the LHS, and substituting the following terms yields the following matrixvector equations, two linear equations in two unknowns 3 and 4 :

49

a b 3 c
d e f

a r3 s3

d r3c3

b r4 s4
c r22 s2 r55 s5

e r4 c4
f r22 c2 r55c5

Step 5. Solve the velocity equations for the unknowns 3 and 4 .

The solution of the above matrix-vector set of equations is:

ce bf
ae bd

af cd
ae bd

With 3 and 4 now known, the end-effector translational velocity B x

from the link2/link3 dyad or the link5/link4 dyad; both yield identical results.

x r s r s
B 2 2 2 3 3 3
y r22c2 r33c3

y can be found either

T

x r s r s
B 5 5 5 4 4 4
y r55c5 r44c4

Planar Five-Bar Parallel Robot Forward Velocity singularity condition

When does the above forward velocity solution fail? At a robot singularity, when the
determinant of the coefficient matrix A goes to zero, the forward velocity solution fails. This case would
require division by zero, yielding infinite 3 and 4 . Lets see what this means physically.

A ae bd (r3 s3 )(r4c4 ) (r4 s4 )(r3c3 ) r3r4 s3c4 r3r4c3 s4 r3r4 sin(4 3 )

Where we used the trig identity sin( a b ) sin a cos b cos a sin b .

A 0 when sin( 4 3 ) 0 , i.e. when:

4 3 0 ,180 ,
Physically, this happens when links 3 and 4 are collinear (straight out or folded on top of each other).
This corresponds to the boundary between the two Forward Pose Kinematics solution branches.

50

4.6.2.2 Inverse Velocity Kinematics

Given r1, 1, r2, r3, r4, r5; angles 2, 3, 4, and 5; plus desired end-effector translational velocity
T
B x y
required actuator angular rates 2 and 5 , plus passive angular rates 3 and 4

Find

The actuator unknowns 2 and 5 can be found independently from the two vector loop-closure
equations presented at the end of the Forward Velocity Kinematics section. Passive unknown angular
rate can be found along with , and passive unknown angular rate can be found along with .
3

The two independent equations for end-effector velocity B x

y are repeated below, written in

T

matrix-vector form:

r2 s2

r2c2

r3s3 2 x

r3c3 3 y

r4 s4
rc
4 4

r5 s5 4 x

r5c5 5 y

And the inverse velocity solutions are:

3 ys
3
xc
r2 sin(3 2 )

5 ys
5
xc
r4 sin(5 4 )

2 ys
2
xc
r3 sin(3 2 )

4 ys
4
xc
r5 sin(5 4 )

Planar Five-Bar Parallel Robot Inverse Velocity singularity condition

When does the above inverse velocity solution fail? At an inverse velocity robot singularity
(different from the forward velocity singularity condition presented earlier), when the determinant of the
coefficient matrix A23 goes to zero, the inverse velocity solution fails. This case would require division
by zero, yielding infinite 2 and 3 . Also, when the determinant of the coefficient matrix A45 goes to
zero, the inverse velocity solution fails. This case would require division by zero, yielding infinite
4

and 5 . Lets see what this means physically.

A23 r2r3 sin(3 2 )

A45 r4r5 sin(5 4 )

A23 0 when sin(3 2 ) 0

A45 0 when sin(5 4 ) 0

i.e. when 3 2 0 ,180 ,

i.e. when 5 4 0 ,180 ,

51

Physically, this happens when links 2 and 3 are collinear (straight out or folded on top of each other).
This corresponds to the boundary between the two Inverse Pose Kinematics solution branches for the
link2/link3 dyad. This also occurs when links 4 and 5 are collinear (straight out or folded on top of each
other). This corresponds to the boundary between the two Inverse Pose Kinematics solution branches
for the link5/link4 dyad.

4.6.3 Acceleration Kinematics

Step 1. The five-bar robot Position and Velocity Analyses must first be complete.
Step 2. Identify the five-bar robot acceleration parameters.

i i (i = 2,3,4,5) is the absolute angular acceleration of link i. 1 1 0 since link 1, the fixed
ground link, cannot rotate.
The velocity equations developed in the Velocity Kinematics section are the starting point for
deriving the acceleration equations.

4.6.3.1 Forward Acceleration Kinematics

Given r1, 1, r2, r3, r4, r5; angles 2, 3, 4, and 5; angular rates 2 ,3 ,4 , 5 ; plus actuator angular
accelerations ,
2

T

x
y , plus passive angular accelerations
Find desired end-effector translational acceleration B
,
3

The acceleration equations come from the first time derivative of the velocity equations as shown below:

r22 s2 r33s3 r55s5 r44 s4

r c r c r c r c
2 2 2

3 3 3

5 5 5

4 4 4

r22 s2 r222c2 r33s3 r332c3 r55 s5 r552c5 r44 s4 r442c4

r c r 2 s r c r 2 s r c r 2 s r c r 2 s
2 2 2

2 2 2

3 3 3

3 3 3

5 5 5

5 5 5

4 4 4

4 4 4

ri 0 , thus the eight XY

Since all links are rigid (i.e. no links are changing lengths), all ri 0 and
pairs of terms above represent the absolute tangential accelerations and absolute centripetal accelerations
at the endpoint of each link.
Gathering unknowns on the LHS, and substituting the following terms yields the following
matrix-vector equations, two linear equations in two unknowns 3 and 4 :

52

a r3 s3
b r4 s4
C r22 s2 r222c2 r332c3 r55 s5 r552c5 r442c4

a b 3 C
d e F

d r3c3
e r4 c4
F r22c2 r222 s2 r332 s3 r55c5 r552 s5 r442 s4

The solution of the above matrix-vector set of equations is:

Ce bF
ae bd

aF Cd
ae bd

T

x
y can be found either
With 3 and 4 now known, the end-effector translational acceleration B
from the link2/link3 dyad or the link5/link4 dyad; both yield identical results.

x r22 s2 r222c2 r33s3 r332c3

y r22c2 r222 s2 r33c3 r332 s3

x r55 s5 r552c5 r44 s4 r442c4

y r55c5 r552 s5 r44c4 r442 s4

Planar Five-Bar Parallel Robot Forward Acceleration singularity condition

When does the above forward acceleration solution fail? The forward acceleration coefficient
matrix is identical to the forward velocity coefficient matrix, which means the singularity conditions are
identical (i.e. when links 3 and 4 are collinear, corresponding to the branch boundary between the two
Forward Pose Kinematics solutions).

4.6.3.2 Inverse Acceleration Kinematics

Given r1, 1, r2, r3, r4, r5; angles 2, 3, 4, and 5; angular rates 2 ,3 ,4 , 5 ; plus desired end-effector
T

x
y
translational acceleration B
Find required actuator angular accelerations 2 , 5 , plus passive angular accelerations 3 , 4
The actuator unknowns 2 , 5 can be found independently from the two vector loop-closure
equations presented at the end of the Forward Acceleration Kinematics section. Passive unknown
angular acceleration 3 can be found along with 2 , and passive unknown angular acceleration 4 can be
T

x
y are
found along with 5 . The two independent equations for end-effector acceleration B
repeated below, written in matrix-vector form:

53

r3s3 2
x r222c2 r332c3

r3c3 3
y r222 s2 r332 s3

r2 s2
rc
2 2

r4 s4
rc
4 4

r5s5 4
x r552c5 r442c4

r5c5 5
y r552 s5 r442 s4

And the inverse acceleration solutions are:

xc3
ys3 r222 cos(3 2 ) r332
r2 sin(3 2 )

xc2
ys2 r332 cos(3 2 ) r222

3
r3 sin(3 2 )

xc5
ys5 r442 cos(5 4 ) r552
r4 sin(5 4 )

xc4
ys4 r552 cos(5 4 ) r442

5
r5 sin(5 4 )

Planar Five-Bar Parallel Robot Inverse Acceleration singularity condition

When does the above inverse acceleration solution fail? The inverse acceleration coefficient
matrices are identical to the inverse velocity coefficient matrices, which means the singularity conditions
are identical (i.e. when links 2 and 3 are collinear or when links 4 and 5 are collinear, corresponding to
the branch boundaries between the two Inverse Pose Kinematics solution branches for the link2/link3
dyad and also for the link5/link4 dyad).

54

4.6.4 Inverse Dynamics

Step 1. The five-bar robot Position, Velocity, and Acceleration Analyses must first be complete.
Step 2. Draw the five-bar robot free-body diagrams (FBDs)

Five-Bar Parallel Robot Free-Body Diagrams (FBDs)

F ij

unknown vector internal joint force of link i acting on link j.

rij

known moment arm vector pointing to the joint connection with link i from the CG of link j.

55

Step 3. State the Problem

Given:

The robot (kinematic parameters r1 ,1 , r2 , r3 , r4 , r5 , masses m2 , m3 , m4 , m5 , center-of-mass vectors

CG2 , CG3 , CG4 , CG5 , mass moments of inertia IGZ 2 , IGZ 3 , IGZ 4 , IGZ 5 ), kinematic motion angles 2 ,3 ,4 ,5 ,
angular velocities 2 , 3 , 4 , 5 , angular accelerations 2 , 3 , 4 , 5 , translational CG accelerations
AG 2 , AG 3 , AG 4 , AG 5 , and external end-effector force F E
Find:
The driving actuator torques 2 and 5 , plus internal joint forces F 21, F 32 , F 43 , F 54 , F15

Count the number of scalar unknowns and the number of scalar equations:

Since this is planar problem there are three scalar dynamics equations per moving link (two
forces XY from Newtons Second Law and one moment Z from Eulers Rotational Dynamics
Equation) and there are four moving links, for a total of 3x4 = 12 scalar equations.

Two vector torques (of one component each) and ten vector internal joint forces (of two
components each) are identified above, for a total of 1+1+2x5 = 12 scalar unknowns.

Therefore, this problem can be solved.

56

Step 4. Derive the Newton-Euler Dynamics Equations

Newton's Second Law
Link 2

Link 3

Link 4

Link 5

F 32 F 21 m2 AG 2

F 43 F 32 F E m3 AG 3

F 54 F 43 m4 AG 4

F 15 F 54 m5 AG5

Euler's Rotational Dynamics Equation

Link 2

Link 3

Link 4

Link 5

G2

2 r 32 F 32 r12 F 21 IG 2 Z 2

G3

r 43 F 43 r 23 F 32 r 43 F E IG 3Z 3

G4

r 54 F 54 r 34 F 43 IG 4 Z 4

G2

5 r15 F 15 r 45 F 54 IG 5Z 5

57

Step 5. Derive the XYZ scalar dynamics equations from the vector dynamics equations.
For each moving link we obtain

Two XY force component equations from Newtons Second Law

One Z moment equation from Eulers Rotational Dynamics Equation

Link 2

F32 X F21 X m2 AG 2 X
F32Y F21Y m2 AG 2Y

2 (r32 X F32Y r32Y F32 X ) (r12 X F21Y r12Y F21 X ) I G 2 Z 2

Link 3

F43 X F32 X m3 AG 3 X FEX

F43Y F32Y m3 AG 3Y FEY

(r43 X F43Y r43Y F43 X ) ( r23 X F32Y r23Y F32 X ) I G 3 Z 3 r43 X FEY r43Y FEX
Link 4

F14 X F43 X m4 AG 4 X
F14Y F43Y m4 AG 4Y

(r54 X F54Y r54Y F54 X ) ( r34 X F43Y r34Y F43 X ) I G 4 Z 4

Link 5

F15 X F54 X m5 AG 5 X
F15Y F54Y m5 AG 5Y

5 (r15 X F15Y r15Y F15 X ) (r45 X F54Y r45Y F54 X ) I G 5 Z 5

58

Step 5. Derive the XYZ scalar dynamics equations (cont.)

Write these XYZ scalar equations in matrix/vector form.

Five-bar robot inverse dynamics 12x12 matrix-vector equation
1
0

r12Y

0
0

0
0

0
0

0
0

1
r12 X
0

0
r32Y
1

1
r32 X
0

0
0
1

0
0
0

0
0
0

0
0
0

0
0
0

0
0
0

0
0

0
r43Y

r23Y

1
r23 X

r43 X

0
0

0
0

0
0

0
0

0
0
0

0
0
0

0
0
0

1
0
r34Y

0
1
r34 X

1
0
r54Y

0
1
r54 X

0
0
0

0
0
0

0
0

0
0

0
0

0
0

0
0

1
0

0
1

1
0

0
1

r45Y

r45 X

r15Y

r15 X

Av b

0 0 F21 X
m2 AG 2 X

0 0 F21Y
m2 AG 2Y

1 0 F32 X
IG 2Z 2

0 0 F32Y
m3 AG 3 X FEX

0 0 F43 X
m3 AG 3Y FEY

0 0 F43Y I G 3 Z 3 r43 X FEY r43Y FEX

0 0 F54 X
m4 AG 4 X

0 0 F54Y
m4 AG 4Y

0 0 F15 X
IG 4Z 4

0 0 F15Y
m5 AG 5 X

0 0 2
m5 AG 5Y

IG5Z5
0 1 5

Step 6. Solve for the unknowns

The coefficient matrix [A] is dependent on geometry (through the moment arms, which are dependent on
the angles from kinematics solutions). The known vector {b} is dependent on inertial terms, gravity,
and the given external forces and moments. {v} is the vector of unknowns.

Solution by matrix inversion

MATLAB

v A b
1

v = inv(A)*b;

% Solution via matrix inverse

Using Gaussian elimination is more efficient and robust to solve for v.

MATLAB

v = A\b;

% Solution via Gaussian elimination

The solution to the unknown internal forces and input torque are contained in the components of v. To
save these values for plotting later, use the following MATLAB code, inside the for i loop.
F21x(i) = v(1);
F21y(i) = v(2);

F15y(i) = v(10);
tau2(i) = v(11);
tau5(i) = v(12);

Terms for the matrix-vector equation

Absolute translational center-of-gravity accelerations

AG 2

AG 2 X RG 2 2 sin 2 RG 222 cos 2

2
AG 2Y RG 2 2 cos 2 RG 22 sin 2

AG 3

AG 3 X r2 2 sin 2 r222 cos 2 RG 3 3 sin 3 RG 332 cos 3

2
2
AG 3Y r2 2 cos 2 r22 sin 2 RG 3 3 cos 3 RG 33 sin 3

AG 4

AG 4 X r5 5 sin 5 r552 cos 5 RG 4 4 sin 4 RG 442 cos 4

2
2
AG 4Y r5 5 cos 5 r55 sin 5 RG 4 4 cos 4 RG 44 sin 4

AG 5

AG 5 X RG 5 5 sin 5 RG 552 cos 5

2
AG 5Y RG 5 5 cos 5 RG 55 sin 5

where RGi ri 2 for uniformly-distributed homogeneous material with regular geometry.

60

Moment arm position vectors

r2

cos 2
r12 X 2

r12

r12Y r2 sin
2
2

r34 X
r 34

r34Y

r2

cos 2
r32 X 2

r 32

r32Y r2 sin
2
2

r4

cos 4
r54 X 2

r 54

r54Y r4 sin
4
2

r3

cos 3

r 23 23 X

r23Y r3 sin
3
2

r5

cos 5

r 15 15 X

r15Y r5 sin
5
2

r 43 43 X
r43Y

r3

cos 3

r3
sin 3
2

r 45

45 X
r45Y

r4

cos 4

r4
sin 4
2

r5

cos 5

r5
sin 5
2

61

4.7 NIST RoboCrane Cable-Suspended Parallel Robot

The NIST RoboCrane4 is a 6-dof, 6-cable-suspended robot that can position and orient its
platform with 6 Cartesian dof. The RoboCrane is classified as an underconstrained cable-suspended
robot since gravity is required in addition to the six active cables in order to try to maintain tension in all
cables.
Like other cable-suspended robots, the RoboCrane shares the advantages of parallel robots vs.
serial robots; in addition, the parallel robot disadvantage of small workspace is wiped out by the
RoboCrane, which can have an arbitrarily-large translation workspace (though a very limited rotational
workspace like other parallel robots). Cable-suspended robots can be lighter, stiffer, and simpler than
other rigid-linked parallel robots. The main disadvantage of the RoboCrane and other cable-suspended
robots is that their cable tensions can only be actuated unidirectionally; in plain English, you cant push
a rope. This poses a significant controls problem plus the ever-present danger of losing robot tension
and stiffness with one or more slack cables.

RoboCrane CAD Model with Serial Arm and Mobile Robots

frc.ri.cmu.edu
4

J. Albus, R. Bostelman, and N. Dagalakis, 1993, The NIST RoboCrane, Journal of National Institute of Standards and
Technology, 10(5): 709-724.

62

RoboCrane Description

The six-dof RoboCrane is capable of XYZ translational and roll-pitch-yaw rotational control of
its moving platform within its workspace. As shown in the kinematics diagrams below, the RoboCrane
is essentially an inverted Stewart Platform parallel robot controlled by six active tensioning cables in
place of the six hydraulic actuators. The side length of the base equilateral triangle is sB and the side
length of the moving platform equilateral triangle is sP. At zero orientations, the moving platform
equilateral triangle is inverted with respect to the base equilateral triangle as shown.
The fixed base Cartesian reference frame is {0}, located on the ground, whose origin is the
center of the base equilateral triangle (from the top view). The base equilateral triangle is located at a
height H above the ground. The three ground-fixed cable connection points are Bi , i 1, 2,3 and the
three moving-platform-fixed cable connection points are Pi , i 1,2,3 . The six cables are connected
between the ground-fixed and platform-fixed cable connection points as shown. The six tensioning
motors for RoboCrane can be mounted on the ground (cables routed via pulleys at the top base frame),
on the top base frame, or on the moving platform itself. The moving platform Cartesian reference frame
is {P}, whose origin is located in the center of the platform equilateral triangle. At zero Euler angles,
the orientation of {P} is identical to that of {0}.
The fixed-base cable connection points Bi are constant in the base frame {0} and the platformfixed cable connection points Pi are constant in the base frame {P}:

sB
2

0
B1 hB
H

0

B 2 lB
H

sB
2

0
B3 hB
H

sP
2

P
P2 hP
0

sP
2

P
P3 hP
0

0

P1 lP
0

where:
hB

3
sB
6

lB

3
sB
3

hP

3
sP
6

lP

3
sP
3

63

RoboCrane Kinematics Diagrams (Top and Front Views)

64

name
sB
sP
H
hB
lB
hP
lP
m

meaning
base equilateral triangle side
platform equilateral triangle side
height from ground to base triangle
planar distance from {0} to near base side
planar distance from {0} to a base vertex
planar distance from {P} to near platform side
planar distance from {P} to a platform vertex
platform mass

Inverse Pose Kinematics (IPK) Solution

The 6-cable RoboCrane robot inverse pose kinematics (IPK) problem is stated: Given the desired
moving platform pose 0PT , calculate the six active cable lengths Li, i 1,2,6 . This IPK model
assumes straight cables (no sag) that are always in tension, and ignores cable mass and elasticity.
The IPK input 0PT may be specified in terms of the desired vector location 0 PP of the origin
of moving frame {P} with respect to {0}, plus three angles representing the orientation of moving frame
{P} with respect to {0}. Choosing --, Z-Y-X Euler Angles (Craig, 2005), the associated orthonormal
rotation matrix is:
c c
R s c

s
0
P

s c c s s
c c s s s
c s

s s c s c
c s s s c

c c

Then the 4x4 homogeneous transformation matrix description of pose is (Craig, 2005):

0 R
0
PT P

0
0
0

0 PP

The solution to the RoboCrane IPK problem may be used as the basis for a pose control scheme,
executing pre-planned trajectories and other motions within its workspace. Like most cable-suspended
robots and many parallel robots in general, the RoboCrane IPK solution is straight-forward and poses no
computational challenge for real-time implementation. Given the desired moving cabin pose 0PT , we
find the moving platform cable connection points Pi. Then the inverse pose solution consists simply of
calculating the cable lengths using the Euclidean norm of the appropriate vector differences between the
various moving and fixed cable connection points. The IPK solution yields a unique closed-form
solution. The moving cable connection points P1, P2, and P3 with respect to the fixed base frame {0}
are:

65

P
0

where the

P
P

T P Pj

j 1,2,3

0
P

vectors were given previously. Note we must augment each position vector above

with a 1 in the fourth row to make the 4x4 matrix multiplication valid. The RoboCrane straight-cable
IPK solution is then:

L1 0 B1 0 P1

L2 0 B1 0 P2

L3

L4

B 2 0 P2

L5 0 B3 0 P3

B 2 0 P3

L6 0 B3 0 P1

Note the direction of the six cable length vectors above were chosen in the direction of positive cable
tensions.

RoboCrane Forward Pose Kinematics (FPK) Solution

The 6-cable RoboCrane forward pose kinematics (FPK) problem is stated: Given the six active
cable lengths Li, i 1,2,6 , calculate the resulting moving platform pose 0PT . The FPK solution for
cable-suspended robots and other parallel robots is generally very difficult. It requires the solution of
multiple coupled nonlinear (transcendental) algebraic equations, from the vector loop-closure equations.
Multiple valid solutions generally result.
Referring to the kinematics diagrams, we see that the RoboCrane FPK problem is identical to the
FPK solution for an upside-down 3-3 Stewart Platform (assuming straight cables always under positive
tension). The FPK solution is based on identifying 3 known triangles, B1P2 B2 , B2 P3 B3 , and B3 PB
1 1.
Construct a virtual link to Pj , perpendicular to base line Bi Bk for each of these three triangles. Imagine
rotating each triangle (each virtual link) about Bi Bk . The FPK solution exists where all three Pj rotate
until P1 P2 , P2 P3 , and P3 P1 are each of the correct, known lengths sP simultaneously. This solution was
presented by Dr. Bob5. The solution boils down to an 8th-order polynomial, meaning that there are
potentially 8 multiple solutions. Even pairs of some of these solutions may be imaginary.

R.L. Williams II, 1992, "Kinematics of an In-Parallel Actuated Manipulator Based on the Stewart Platform Mechanism",
NASA Technical Memorandum 107585, NASA Langley Research Center, Hampton, VA.

66

Figure 8. RoboCrane FPK Diagram

RoboCrane Pseudostatic Analysis

To maintain safe and stable control in all motions, all cable tensions must remain positive at all
times. Gravity acting on the moving platform is required to ensure that the six active cables remain in
tension, as long as the rotations are not too far from the nominal horizontal orientation. A pseudostatic
model is developed in this section and applied to the RoboCrane inverse statics solution.
Equations for Static Equilibrium

This section presents statics modeling for the six-cable-suspended RoboCrane robot. All six
active cables connect in parallel from the fixed base to the moving platform, as shown in the kinematics
diagrams. Again we assume straight cables (no sag) that are always in tension, and ignore cable mass
and elasticity. In pseudostatics it is assumed that the focus cabin accelerations and velocities are low
enough to justify ignoring inertial dynamic effects and use statics equations of equilibrium.
For static equilibrium the vector force sum and vector moment sum of the six active cable
tensions plus gravitational loading and external wrench acting on the focus cabin must balance to zero.
The statics free-body diagram for the moving platform is shown below, where CG indicates the center of
gravity location. The six active cable tension vectors are ti, i 1,2,6 .

67

RoboCrane Statics Free-Body Diagram

The vector force and moment equations of static equilibrium are:

6

t
i 1

m
i 1

mg FEXT 0

P0 R P PCG mg M EXT 0

where t i ti L i is the vector cable tension applied to the moving platform by the ith active cable (in the
positive cable length direction L i as established in IPK); m is the moving platform mass;

g 0 0 g is the gravity vector; FEXT is the external vector force exerted on the moving platform
T

by the environment; m i P0 R P P j t i is the moment due to the ith active cable tension ( P Pj is the
moment arm from the moving platform control point P to the jth active cable connection point, expressed
in {P} coordinates); P PCG is the position vector to the moving platform CG from the moving platform
control point P (the origin of {P}); and M EXT is the external vector moment exerted on the moving
platform by the environment. Moments are summed about the moving platform control point P and all
vectors must be expressed in a common frame; {0} is chosen.
Now we derive the pseudostatics Jacobian matrix based on the vector force and moment statics
equations. Substituting the above details into the static equilibrium equations yields:

S t WEXT G

68

where t t1 t2 t6 is the vector of active cable tensions, G m g

T

gravity wrench vector, WEXT FEXT

0
P

R P PCG m g is the
T

M EXT is the external wrench vector, and the statics Jacobian

T

matrix S is:

L
1

L
2

P1 L1 P2 L2

L 3

L
4

P2 L 3 P3 L 4

L 5

P3 L
5

L 6

P1 L
6

where Pj 0 Pj P0 R P Pj , j 1,2,3 .

RoboCrane Inverse Pseudostatics Solution

The statics equations can be used in two ways. Given the active cable tensions t and the six
cable unit vectors L i from kinematics analysis, forward statics analysis uses the equations of static
equilibrium directly to verify statics equilibrium. For control, simulation, and valid-tension workspace
determination, the more useful problem is inverse statics analysis. This problem is stated: calculate the
required active cable tensions t given the focus cabin mass and pose, plus all L i . It is solved by
inverting the static equilibrium equations:

t S WEXT G
1

The statics Jacobian Matrix S is a square 6x6 matrix and hence the standard matrix inverse
applies in (11). A unique t solution is guaranteed if the RoboCrane robot is not in a singular pose.
Assuming pseudostatic motion, the inertia of the actuator shafts do not enter into the analysis and
the statics torque/tension relationship for each of the 6 actuators is i rti i , i 1,2,6 , where i is the
ith actuator torque, ri is the ith cable reel radius, and ti is the ith cable tension.

69

RoboCrane Examples
For these examples, the RoboCrane dimensions are sB = 10 m, sP = 3 m, H = 8 m, and mP = 100
T
kg. All examples assume the platform CG is located at the origin of {P} so that P PCG 0 0 0 .

Also there is zero external wrench WEXT .

Snapshot Example
Given

P 1
0

2 3 m and
T

10 6 4 , the calculated IPK and inverse

statics results are:

L 7.080 8.313 6.203 5.777 8.494 8.711 m

T

t 325.1

125.2 318.6 352.4 76.1 123.8 N

T

70

Trajectory Example
T
The moving platform control point {P} traces a circle of center 0 0 4 and radius 2 m, with
zero orientations 0 0 0 . At the same time, the Z displacement goes through 2 entire
sine wave motions centered on Z = 4 m with a 1 m amplitude.

Circular Trajectory

Cable Lengths

Cable Tensions

71

4.8 Delta Parallel Robot Inverse and Forward Kinematics

Clavels 3-RUU Delta Robot6 is arguably the most successful commercial parallel robot to date.
The left image below shows the original design from Clavels U.S. patent7, and the right photograph
below shows one commercial instantiation of the Delta Robot.

Delta Robot Design1

ABB FlexPicker Delta Robot

www.abb.com

The Delta Robot has 4-degrees-of-freedom (dof), 3-dof for XYZ translation, plus a fourth inner
leg to control a single rotational freedom at the end-effector platform (about the axis perpendicular to the
platform). The remainder of this document will focus only on the 3-dof XYZ translation-only Delta
Robot since that is being widely applied by 3D printers and Arduino hobbyists.
Presented is a description of the 3-dof Delta Robot, followed by kinematics analysis including
analytical solutions for the inverse position kinematics problem and the forward position kinematics
problem, and then examples for both, snapshots and trajectories. The revolute-inputs 3-RUU and the
prismatic inputs 3-PUU Delta Robots are both covered.
This section is presented on-line:
www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf

6
7

R. Clavel, 1991, Conception d'un robot parallle rapide 4 degrs de libert, Ph.D. Thesis, EPFL, Lausanne, Switzerland.
R. Clavel, 1990, Device for the Movement and Positioning of an Element in Space, U.S. Patent No. 4,976,582.

72

4.9 Stewart Platform

The 6-UPS Stewart-Gough Platform, shown below, has seen significant practical applications
from flight simulators and entertainment, to haptic interfaces and general parallel robots.

6-dof Stewart Platform Parallel Robot

Please see the following link for Dr. Bobs NASA Technical Memorandum presenting the
kinematics of the Stewart Platform:
www.ohio.edu/people/williar4/html/PDF/StewartPlatform.pdf

73

5. Manipulator Dynamics
Kinematics is the study of motion without regard to forces.
Dynamics is the study of motion with regard to forces. It is the study of the relationship between
forces/torques and motion. Dynamics is composed of kinematics and kinetics.
a) Forward Dynamics (simulation) given the actuator forces and torques, compute the
resulting motion (this requires the solution of highly coupled, nonlinear ODEs): Given {}, calculate
, , (all are N x 1 vectors).

b) Inverse Dynamics (control) given the desired motion, calculate the actuator forces and
torques (this linear algebraic solution is much more straight-forward than Forward Dynamics): Given
, , , calculate {} (all N x 1 vectors).

Both problems require the N dynamic equations of motion, one for each link, which are highly coupled
and nonlinear. There are two basic methods for deriving the dynamic equations of motion.
Newton-Euler recursion (force balance, including inertial forces with D'Alembert's principle).
Lagrange-Euler formulation (energy method).
Kinetics
Translational

Newton's Second Law

Inertial force at center of mass

Rotational

Euler's Equation
Inertial moment anywhere on body

The kinematics terms aCi , i , i must be moving with respect to an inertially-fixed frame.
The frame of expression {k} needn't be an inertially-fixed frame.
Assumptions
serial robot
rigid links
ignore actuator dynamics
no friction
no joint or link flexibility

74

5.1 Inertia Tensor (Mass Distribution)

The inertia tensor is a spatial generalization of the planar scalar moment of inertia. Its units are
mass times distance2 (kg-m2). The symmetric inertia tensor expressed at a given point A in the rigid
body, relative to frame {A} is:

I xx

I I xy
I xz

I xy
I yy
I yz

I xz

I yz
I zz

Mass moments of inertia

I xx y 2 z 2 dv

I yy x 2 z 2 dv

I zz x 2 y 2 dv

Mass products of inertia

I xy xy dv

I xz xz dv

I yz yz dv

Principal moments of inertia

A certain orientation of the reference frame {A}, the principal axes, yields zero products of
inertia. The invariant eigenvalues of a general [AI] are the principal moments of inertia, and the
eigenvectors are the principal axes.
More interesting facts regarding inertia tensors
1)
If two axes of the reference frame form a plane of symmetry for the mass distribution, the
products of inertia normal to this plane are zero.
2)
Moments of inertia must be positive, products of inertia may be either sign.
3)
The sum of the three moments of inertia are invariant under rotation transformations.
Parallel-axis theorem
We can obtain the mass moment of inertia tensor at any point {A} if we know the inertia tensor
T
at the center of mass {C} (assuming these two frames have the same orientation). PC xC yC zC
is the vector giving the location of the center of mass {C} from the origin of {A}.
Here are some example inertia tensor components using the parallel axis theorem.
A
I zz C I zz m xC2 yC2
A

I xy C I xy mxC yC

Here is the entire inertia tensor expressed in vector-matrix form using the parallel axis theorem.
T
T
AI C I m PC PC I3 PC PC

75

5.2 Newton-Euler Recursive Algorithm

This is a recursive approach (Craig, 2005) based on free-body diagrams (FBDs) to determine the
dynamics relationships link-by-link. Used numerically it can calculate the inverse dynamics solution
efficiently.
Free-body diagram of link i

fi
ni

internal force exerted on link i by link i-1.

internal moment exerted on link i by link i-1.

Inertial loads Newton and Euler translational and rotational dynamics equations

Force balance

Moment balance (about CGi) (using D'Alemberts principle, the inertial force is m{a}).

76

Newton-Euler Recursive Algorithm Summary

This methods can be used to find the robot dynamics equations of motion. It can also be used to
directly solve the inverse dynamics problem numerically. The summary of equations below, from Craig
(2005), assume an all revolute-joint manipulator (prismatic joint dynamics have different equations).
Outward iteration for kinematics i : 0 N 1
(without regard for frames of expression, for clarity)
Velocities and accelerations (kinematics)

i 1 i Zi 1i 1

i 1 i i Zi 1i 1 Zi 1i 1

ai 1 ai i i Pi 1 i i i Pi 1

aCi 1 ai 1 i 1 i 1PCi 1 i 1 i 1 i 1PCi 1

Inertial loading (kinetics)

Fi1 mi 1 aCi 1
Ni1 C I i 1 i1 C I i 1
Inward iteration for kinetics
i:N 1
(without regard for frames of expression, for clarity)
Internal forces and moments

fi fi1 Fi
ni ni 1 i PCi Fi i Pi 1 fi1 Ni
Externally applied joint torques
i ni Z i

Inclusion of gravity forces

a g
0

This is equivalent to a fictitious upward acceleration of 1g of the robot base, which accounts for
the downward acceleration due to gravity (i.e. this conveniently includes the weight of all links).

77

5.3 Lagrange-Euler Energy Method

This is an alternative method to find the robot dynamics equations of motion. It requires only
translational and rotational link velocities, not accelerations. The Lagrangian is formed from the
kinetic energy k and potential energy u of the robot system.

) u ()
L k u k (,
1
1
T
T
mi VCi VCi i Ci I i i
2
2
0
0
ui uREF mi g PCi

ki

) k
k (,
i

u () ui

i 1

i 1

Note

) 1
T M ()

k (,
2

where M ( ) is the manipulator mass matrix.

Dynamic equations of motion

These are found for each active joint from the following expression involving the Lagrangian,
joint variable, and actuator torque. Perform this equation N times, once for each joint variable i, to yield
N independent dynamics equations of motion.
d L L
i

dt i i

) u ( ) .
This expression may be rewritten using L k (,
d k

dt i

k u

i
i

78

5.4 Simple Dynamics Example

Derive the dynamic equation of motion for a planar one-link 1R mechanism by three methods:
1) Sophomore dynamics method FBD, force and moment dynamics balance.
2) Newton-Euler recursion.
3) Lagrange-Euler formulation.

g
Y

1) Sophomore methods - FBD, force balance

FY
-FX

Free-Body Diagram

mg

FX

-FY

79

1) Sophomore methods FBD, force balance (cont.)

maCx

L
L

Fx m s 2 c
2
2

mL
0
( s 2 c )
Fx
2
0

a)

b)

c)

maCy

L
L

Fy mg m c 2 s
2
2

L
0
Fy mg m (c 2 s )
2

z0

0 I

0 I

mL
gc
2

mL
gc
2

where

I C I md 2 C I zz

mL2
4

80

Simple Dynamics Example (cont.)

2) Newton-Euler recursion
Outward iteration i = 0 (the only iteration)

L
2

1PC1 0
0

0

P1 0
0

0

0 0
0

a
1

1
0

c
a1 s
0

1
0

c
R s
0

0

0 0

0

s
c
0

C1

0
I yy
0

0
0
I zz

P P a
0

s
c
0

I xx
I1 0
0

C1

0

a0 g to account for gravity

0

C1

L 2

gs 2

1
1
F1 m aC1 m gc 2

0 0 gs

0 g gc
1 0 0

a P P
1

0
0
1

C1

L 2

gs 2

1
a1 gc
2

N1 I1 1 1 I1 1 0

I zz
C1

C1

81

Inward iteration i = 1 (the only iteration)

0

f 2 0

0

0

n2 0

0

L 2

gs 2

1
1
2
1

f1 2 R f2 F1 m gc 2

n
1

1
2

R 2 n2 1PC1 1F1 1P2 21R 2 f 2 1N1

0
0

0
1n1
0
mL
L I zz

gc

2
2

1n1 1Z1

I zz

mL2 mL
gc

4
2

Check with the sophomore dynamics method above.

c
0 f1 10 R 1 f1 s
0

s
c
0

L 2
L 2
L

gs 2
2 c 2 s
0

L
L

L 2

0 m gc m s c g
2
2
2

1
0
0

82

3) Lagrange-Euler formulation
0
1 0

I xx
I1 0

0
C1

0
I yy
0

0
0
I zz

Kinetic and potential energy are scalars invariant with frame of expression write k in {1} and u in {0}.

v
1

C1

0
L

2
0

0 0
1 I1 1 0 0
I
zz
1

C1

L
2 c
0

L
L
u 0 m g s mg s
2
0 2
0

1 mL2 2 1 2
I zz
2 4
2

1
mL2
L
L k u 2 I zz
mg s
2
4
2
d L L

dt

L
mL2

I
zz

d L
mL2

zz

4
dt

mL2

L
L
mg c

mL

I zz
gc

4
2

This result agrees with the sophomore and Newton-Euler recursion dynamics methods above to solve
the same problem.

83

5.5 Structure of Manipulator Dynamics Equations

State Space Equation

M () V (, ) G()

M ( )

N N mass matrix; symmetric and positive definite

V (, )

N 1 vector of Coriolis and centripetal terms

G ()

N 1 vector of gravity terms

Configuration Space Equation

C ()
2 G ()
M () B()

M ( )

N N mass matrix; symmetric and positive definite

C ( )

N N 1
Coriolis matrix
2
N N 1
1
12 13 N 1N
2
N N centripetal matrix

N 1

G ()

N 1 vector of gravity terms

B ( )

2
1

22 N2

Cartesian State Space Equation

F M x () X Vx (, ) Gx ()

M x ()

N N Cartesian mass matrix; symmetric and positive definite

V (, )

N 1 vector of Cartesian Coriolis and centripetal terms

Gx ()

N 1 vector of gravity terms in Cartesian space

where

M x () J

M () J
Vx (, ) J T V (, ) M () J 1 J
T

Gx () J T G ()

note

X J
X J J

84

5.6 Robot Dynamics Example

Here we summarize the dynamics equations of motion for the two-link planar 2R robot when
each link is modeled as a homogeneous rectangular solid of dimensions Li, hi, wi of mass mi.

L2

Y0

L1

X0

Newton-Euler recursion with outward kinematics and inertial calculations, followed by inward
kinetics balances yields.

2 I zz 2

m2 L1 L2 c2 m2 L22

2
4

m2 L22
1 I zz 2
4

m2 L1 L2 s2
2
2

2 m2 L2 c12
1

m1 L12
m2 L22
m2 L1 L2 c2 m2 L22
2
1 I zz1 I zz 2
m2 L1 m2 L1 L2 c2

1 I zz 2
4
4
2
4

mLc
m LL s
m Lc
2 1 2 2 22 m2 L1 L2 s2 12 1 1 1 m2 L1c1 2 2 12 g
2
2

where I zzi

mi 2 2
( Li hi ) .
12

These dynamics equations of motion are very complicated imagine how much worse these
equations will be for a spatial 6-axis robot such as the PUMA industrial robot.

85

State Space Representation

For the planar 2R robot, the dynamics equations of motion from the previous page are expressed
in state-space form below:
1

M () V (, ) G()

m1 L12
m2 L22
2

I
I
m
L
m
L
L
c
2 1
2 1 2 2
zz1 zz 2
4
4
M ()
2
m LLc m L

I zz 2 2 1 2 2 2 2

2
4

I zz 2

m2 L1 L2 c2 m2 L22

2
4
m L2

I zz 2 2 2

m2 L1 L2 s2 2

2 m2 L1 L2 s2 1 2

2

V (, )

m2 L1 L2 s2 2

m2 L2 c12
m1 L1c1
2 m2 L1c1
2
G()
g
m
L
c
2
2
12

2
Configuration Space Representation

For the planar 2R robot, the dynamics equations of motion from the previous page are expressed
in configuration-space form below.

C ()
2 G ()
M () B()

M () ,G() same as above

m2 L1L2 s2
12
0

B()

(
)

m L L s
2 1 2 2
2

m2 L1L2 s2
12
2
2
2
0

86

Numerical Dynamics Example

For the planar 2R robot whose dynamics equations of motion were presented analytically
above, here we calculate the required torques (i.e. solve the inverse dynamics problem) to provide the
commanded motion at every time step in a resolved-rate control scheme. Identical results are obtained
by both analytical equations and numerical Newton/Euler recursion.
Given

L1 = 1.0 m
L2 = 0.5 m
Both links are solid steel with mass density = 7806 kg m 3 and both have width and thickness
dimensions w = t = 5 cm. The revolute joints are assumed to be perfect, connecting the links at their
very edges (not physically possible, just a simplified model).
The initial robot configuration is
1 10

2 90
The constant commanded Cartesian velocity is
0

x
0
X y 0.5

(m/s)

The dynamics equations require the relative joint accelerations ; how do we find these? (See
the last page of the Acceleration Kinematics section in this ME 4290/5290 Supplement:
T
J 1 X J . In this example, X 0 0 .)

Simulate motion for 1 sec, with a control time step of 0.01 sec. The plots for various variables of
interest (joint angles, joint rates, joint accelerations, joint torques, and Cartesian pose) for this problem
are given on the following page.
In the last plot, note that the robot travels 0.5 m in the Y0 direction in 1 sec (which agrees with
the constant commanded rate of 0.5 m/s). The robot does not move in X; must move to compensate
for the pure Y motion, but we cannot control independently with only two-dof. The first three plots are
kinematics terms related to the resolved-rate control scheme; they are inputs to the inverse dynamics
problem. The joint torques are calculated by the numerical recursive Newton-Euler inverse dynamics
algorithm. These are the joint torques necessary to move this robots inertia in the commanded manner.
Notice from the joint angles, joint rates, joint accelerations, and joint torques plots that the robot is
approaching the 2 = 0 singularity towards the end of this simulated motion.

87

Numerical Dynamics Example: Plots

Cartesian Pose

Joint Angles

90
80

1.8
x (r), y (g), (b) (m,rad)

70
(g) (deg)

1.6

60
50

(r),

1.4

40

1.2

30

20

0.8
0.6
0

10
0.2

0.4
0.6
time (sec)

0.8

0
0

0.2

Joint Rates

0.4
0.6
time (sec)

0.8

0.8

Joint Accelerations

1.5

10

joint 1 (r), joint 2 (g) (rad/s )

0.5

joint 1 (r), joint 2 (g) (rad/s)

0
-0.5
-1
-1.5
-2

0
-5
-10
-15

-2.5
-3
0

0.2

0.4
0.6
time (sec)

0.8

-20
0

0.2

Joint Torques

Joint Torques

80

250
200

60

(1-red; 2-green) (Nm)

(1-red; 2-green) (Nm)

70

50
40
30
20

150
100
50
0

10
0
0

0.4
0.6
time (sec)

0.2

0.4
0.6
time (sec)

0.8

Dynamics Results Ignoring Gravity

-50
0

0.2

0.4
0.6
time (sec)

0.8

Dynamics Results Including Gravity

88

6. Robot Control Architectures

With the robotic kinematics and dynamics we have learned and simulated, we can simulate robot
control using various popular robot control architectures.

6.1 Inverse Pose Control Architecture

6.2 Inverse Velocity (Resolved-Rate) Control Architecture

89

6.3 Inverse Dynamics Control Architecture

The inverse dynamics method is also called the computed torque control method, and it is also
called the feedback linearization control method.
In Chapter 4 we learned that the robot dynamics equations of motion are highly coupled and
nonlinear. Here they are in matrix/vector form.

M () V (, ) F (, ) G ()
{

vector of applied joint torques

M ( )

N N mass matrix; symmetric and positive definite

V (, )

N 1 vector of Coriolis and centripetal terms

F (, )

N 1 vector of friction torques

G ()

N 1 vector of gravity terms

, ,

N 1 joint angles, rates, and accelerations vectors

Computed Torque Control Architecture

The computed-torque control method is based on canceling the dynamics effects by using the
inverse dynamics method, in order to linearize the robot system for standard controller methods, such as
PID control.

90

6.4 NASA Langley Telerobotics Resolved-Rate Control Architecture

Below is pictured the Intelligent Systems Research Laboratory (ISRL) at NASA Langley
Research Center, circa 1992.

Force-Reflecting Operator Station

Dual PUMA Robots with Space Station Task

NASA Langley Research Center Telerobotics Laboratory (ISRL)

Below is shown the overall resolved-rate-based robot control architecture developed at NASA
Langley Research Center in the Automation Technology Branch, with Dr. Bob on the team, in the early
1990s. This controller architecture was novel and is still unique in robot control.

NASA Langley Research Center Telerobotic Controller Architecture

91

In the top, forward control path above we see that resolved-rate (inverse velocity) is the basic
control method, as opposed to inverse pose control. Being a linear problem, the resolved-rate method
has several attractive benefits over IPK control including the ability to add velocity control inputs as
vectors, which is not possible with IPK (due to orientation).
The total Cartesian velocity vector (translational and rotational) input determining the robot
motion is the sum of four inputs, from the hand controller, position controller, vision controller, and
force/torque controller. The displacement of the hand controller is interpreted as a velocity input, for
both the translational and rotational modes. Often the joystick will have a hardware or software spring
return-to-center, including deadbands, to more easily turn off velocities after certain directions are
finished for the time being. The position, vision, and force controllers (details hidden) yield a Cartesian
velocity that will move the robot in the direction that the control algorithm determines in each case.
Not all control modes need be enabled at all times, but all can be combined is desired. This
could lead to controller contention. If the joystick enables force reflection, this provides an extra
sensory feedback back to the human operator which has been proven very effective in many experiments
in different research labs. Today we call this force-reflection aspect of telerobotics haptics.

92

6.5 Naturally-Transitioning Rate-to-Force Controller Architecture

Resolved-rate control automatically (naturally) changes to force control when the robot endeffector enters into contact with the environment. A force/torque (F/T) sensor and force/moment
accommodation (FMA) algorithm is required to accomplish this. After contact, the constant velocity
command becomes a constant force command. In addition, if there is a human teleoperator with haptic
interface, the force/moment that the human applies to the interface becomes proportional to the
force/moment that the robot exerts on the environment at contact. This works and feels great in realworld applications, providing telepresence.

XC +

XM

Master

Resolved
Rate

XA
Manipulator
F/T
Sensor

XF

FM

JT
(FMA)

KF

(Force Reflection - Optional)

This control architecture was implemented by Dr. Bob and teams at NASA Langley Research
Center and Wright-Patterson AFB 8.
6F

8
R.L. Williams II, J.M. Henry, M.A. Murphy, and D.W. Repperger, 1999, Naturally-Transitioning Rate-to-Force Control in Free and
Constrained Motion, ASME Journal of Dynamic Systems, Measurement, and Control, Trans. ASME, 121(3): 425-432.

93

6.6 Single Joint Control

Every controller architecture weve considered requires linearized independent (but
simultaneous) single joint angle control, presented in this section.
Manipulator dynamics is extremely complicated considering the number of terms. We can easily
use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to
understand. In this case, numerical Newton-Euler recursion ala Craig (2005, Chapter 6) is useful to get
around the need for analytical expressions.
How is manipulator dynamics done in industry for control purposes? In almost all cases it is
ignored. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion of
one link on its neighbors we will see this in modeling soon.) The vast majority of multi-axis industrial
robots are controlled via linearized, independent single joint control. So, robot control in industry is
generally accomplished by using N independent (but simultaneous) linearized joint controllers, where N
is the number of joint space freedoms of the robot.
We now will briefly discuss single joint control. We will focus on a common system, the
armature-controlled DC servomotor driving a single robot joint / gear train / link combination, shown
below. This requires dynamics, but it is not coupled nor nonlinear.
Open-Loop System Diagram

V(t)
L
R
iA(t)
vB(t)

armature voltage
armature inductance
armature resistance
armature current
back emf voltage

M(t)
M(t)
M(t)
JM
CM
n

generated motor torque

motor shaft angle
motor shaft velocity
lumped motor inertia
motor viscous damping
gear ratio

L(t) load torque

L(t) load shaft angle
L(t) load shaft velocity
JL(t)
CL

total load inertia

load viscous damping

94

Closed-Loop Feedback High-level Control Diagram

High-level Computer Control Diagram

Simplifying Assumptions
rigid motor, load shafts
n >> 1
(the large gear ratio n reduces speed and increases torque)
Real-world vs. model characteristics
Real World

Our Model (simplified)

nonlinear

linearized

multiple-input, multiple output (MIMO)

single-input, single output (SISO)

coupled

decoupled

time-varying load inertia

treat as a disturbance; the large gear ratio

diminishes this problem
hysteresis, backlash, stiction, Coulomb friction ignore
discrete and continuous

continuous for control design

95

Single Joint/Link System Modeling

We must derive all linear ordinary differential equations (ODEs) describing the dynamics of an
armature-controlled DC servomotor driving a single robot joint / link. This is an electromechanical
system, including a gear train. We must perform modeling, simulation, and control. The system
diagram was shown earlier in this section.
Armature Circuit Diagram

Electrical Model

Kirchoffs voltage law

(1)

Electromechanical coupling
The generated motor torque is proportional to the armature current

(2)
The back-emf voltage is proportional to the motor shaft angular velocity
(3)

(KT = KB can be shown)

96

Mechanical Model
Eulers Equation (the rotational form of Newtons Second Law):

Free-body diagrams

Load
(4)

Motor
(5)
where LR ( t ) is the load torque reflected to the motor shaft.

Gear ratio

(6)

Substituting (6) into (5) yields (7)

(7)

97

Reflect load inertia and damping to motor shaft

Substitute

into (4)
(8)

Substitute (8) into (7) to eliminate L

(9)
Combine terms
(10)
define

JL
n2
C
CE CM 2L
n
JE JM

effective inertia, total reflected to motor shaft

effective damping, total reflected to motor shaft

98

Final Mechanical Model (ODE)

(11)

This is a second-order ODE in M. Can also be written as first-order ODE in M.

(12)

1
is small; therefore we can choose a nominal JL and
n2
assume it is constant without much error in control.
For common industrial robots, n >> 1 so

For example, the NASA 8-axis ARMII robot downstairs has n = 200 with harmonic gearing.
This is why we can ignore the time-varying robot load inertia and design decoupled independent linear
joint controllers. The gear-reduced load inertia variation is then treated as a disturbance to the single
joint controller.

An alternative is to reflect the motor inertia and damping to the load shaft as shown below, but
we will use the first case above.
( J L n 2 J M )L (C L n 2 C M )L L

99

Open-Loop Block Diagram

Use Laplace transforms on different ODE pieces of the system model and then connect the
components together in blocks connected by arrows representing variables. A transfer function goes
inside each block representing component dynamics. This block diagram is shown below, in
MATLAB/Simulink implementation, including a disturbance torque.

Closed-Loop Feedback Control Diagram

Assuming perfect encoder sensor for angular position feedback, we include block for the singlejoint PID controller. The MATLAB/Simulink implementation is shown below.

100

PID Controller Design

PID Controller characteristics

PID stands for:

Controller transfer function:

Each term does this:

Use PID w/ approximate derivate in Simulink numerical differentiation can lead to errors and
numerical instability and thus it is to be avoided if possible.
Trial and Error PID Design

Start with the P gain value start low and increase P to get the desired time nature of response.
Then add the D gain value term for stability. Add the I gain value to reduce steady-state error. Always
use Simulink to simulate control and dynamics response of the single joint/link system for different PID
choices; compare various cases and select a suitable controller in simulation.
A better approach is to perform analytical design for PID controllers using classical control
theory, such as in ME 3012.
Controller Performance Criteria
Stability
Rise Time
Peak Time
Percent Overshoot
Settling Time
Steady-state error

These performance criteria provide a rational basis for choosing a suitable controller, at least in
theory and simulation. The real world always provides some additional challenges (noise, modeling
errors, nonlinearities, calibration, backlash, etc.).

101

Include Gravity as a Disturbance Torque

Unmodeled, unexpected disturbances are modeled as disturbances in control systems. It is

convenient to do so at the motor torque level in the block diagram.

Gravity is known and expected. However, in single joint control we can include the effect of gravity as
a disturbance. First let us model the gravity effect for a single joint. Lump all outboard links, joints, and
motors as a single rigid body.

test with original PID gains

redesign new PID gains with gravity disturbance considered