Forward Kinematics and ConfigurationsKris HauserI400/B659: Intelligent RoboticsSpring 2014

Articulated Robot• Robot: usually a rigid

articulated structure• Geometric CAD models,

relative to reference frames

• A configuration specifies the placement of those frames (forward kinematics)

q1

q2

Forward Kinematics

• Given:• A kinematic reference frame of the robot• Joint angles q1,…,qn

• Find rigid frames T1,…,Tn relative to T0

• A frame T=(R,t) consists of a rotation R and a translation t so that T·x = R·x + t• Make notation easy: use homogeneous coordinates• Transformation composition goes from right to left:

T1·T2 indicates the transformation T2 first, then T1

Kinematic Model of Articulated Robots: Reference Frame

T0

L0

L1

L2 L3

T1ref

T2ref

T3ref

T4ref

Rotating the first joint

T0

L0

T1ref

q1

T1(q1)

T1(q1) = T1ref·R(q1)

Where is the second joint?

T0

T2ref

q1

T2(q1) ?

Where is the second joint?

T0

T2ref

q1

T2parent(q1) = T1(q1) ·(T1

ref)-1·T2ref

After rotating joint 2

T0

T2R

q1

T2(q1,q2) = T1(q1) ·(T1ref)-1·T2

ref·R(q2)

q2

After rotating joint 2

T0

T2R

q1

Denote T2->1ref = (T1

ref)-1·T2ref (frame relative to parent)

T2(q1,q2) = T1(q1) ·T2->1ref·R(q2)

q2

General FormulaDenote (ref frame relative to parent)

T0

L0

L1

L2L3

T1(q1)

T2(q1,q2)T3(q1,..,q3)

T4(q1,…,q4)

Generalization to tree structures• Topological sort: p[k] = parent of link k• Denote (frame i relative to parent)• Let A(i) be the list of ancestors of i (sorted from root to i)

To 3D…• Much the same, except joint axis must be defined (relative to

parent)• Angle-axis parameterization

Generalizations• Prismatic joints• Ball joints• Cylindrical joints• Spirals• Free-floating bases

From LaValle, Planning Algorithms

Configuration Space

q=(q1,…,qn)

q1q2

q3

qn

A robot configuration is a specification of the positions of all robot frames relative to a fixed coordinate system

Usually a configuration is expressed as a “vector” of parameters

reference point

Rigid Robot

• 3-parameter representation: q = (x,y,q)• In a 3-D workspace q would be of the form

(x,y,z,a,b,g)

x

yq

robot reference direction

workspace

Articulated Robot

q1

q2

q = (q1,q2,…,q10)

Protein

Configuration Space• Space of all its possible configurations• But the topology of this space is in general not that of a

Cartesian space

yq

robot

x

q

q’

3-D cylinder embedded in 4-D space

x

y

q2p

S1

R2S1

Configuration Space• Space of all its possible configurations• But the topology of this space is in general not that of a

Cartesian space

C = S1 x S1

Configuration Space• Space of all its possible configurations• But the topology of this space is in general not that of a

Cartesian space

C = S1xS1

Configuration Space• Space of all its possible configurations• But the topology of this space is in general not that of a

Cartesian space

C = S1xS1

Some Important Topological Spaces• R: real number line• Rn: N-dimensional Cartesian space• S1: boundary of circle in 2D• S2: surface of sphere in 3D• SO(2), SO(3): set of 2D, 3D orientations (special orthogonal group)• SE(2), SE(3): set of rigid 2D, 3D translations and rotations (special

Euclidean group)• Cartesian product A x B, power notation An = A x A … x A• Homeomorphism ~ denotes topological equivalence

• Continuous mapping with continuous inverse (bijective)• Cube ~ S2

• SO(2) ~ S1

• SE(3) ~ SO(3) x R3

What is its topology?

q1

q2

(S1)7xI3

(I: Interval of reals)

Notion of a (Geometric) Path

• A path in C is a piece of continuous curve connecting two configurations q and q’:

t : s [0,1] t (s) C

q1

q3

q0

qn

q4

q2

t(s)

Examples• A straight line segment linearly interpolating between a and b

• t(s) = (1-s) a + s b• What about interpolating orientations?

• A polynomial with coeffients c0,…,cn

• t(s) = c0 + c1s + … + cnsn

• Piecewise polynomials• Piecewise linear• Splines (B-spline, hermite splines are popular)

• Can be an arbitrary curve• Only limited by your imagination and representation capabilities

Notion of Trajectory vs. Path

• A trajectory is a path parameterized by time: t : t [0,T] t (t) C

q1

q3

q0

qn

q4

q2

t(t)

x

yq

reference point

robotreference direction

workspaceq

x

y

2p

configuration space

What is the placement of the robot in the workspace at configuration (0,0,0)?

Translating & Rotating Rigid Robot in 2-D Workspace

x

yq

reference point

robotreference direction

workspaceq

x

y

2p

configuration space

What is the placement of the robot in the workspace at configuration (0,0,0)?

Translating & Rotating Rigid Robot in 2-D Workspace

x

yq

reference point

robotreference direction

workspaceq

x

y

2p

configuration space

What is this path in the workspace?

What would be the path in configuration space corresponding to a full rotation of the robot about point P?

P

Translating & Rotating Rigid Robot in 2-D Workspace

Klamp’t Python API• world = WorldModel()• world.readFile([some file])• robot = world.robot(0) [if the world has only one robot]• A robot’s configuration is a list of numbers

• robot.getConfig()• robot.setConfig(q) automatically performs forward kinematics• It does not necessarily transform like a vector!• Robot-specific interpolation function: robot.interpolate(a,b,u)

• A robot’s frames are given as a list of RobotModelLink’s• link = robot.getLink([index or name])• (R,t) = link.getTransform()

• 3D rigid transform utilities in se3.py