1. Forward Kinematics and Configurations
Kris Hauser
I400/B659: Intelligent Robotics
Spring 2014 1

2. 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

3. 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

4. Kinematic Model of Articulated Robots: Reference Frame
T2ref
T3ref L2
T4ref T0 L3
T1ref L1 L0

5. Rotating the first joint
T1(q1) = T1ref·R(q1) q1
T1(q1) T0
T1ref L0

6. Where is the second joint?
T2(q1) ?
T2ref T0 q1

7. Where is the second joint?
T2ref T0 q1
T2parent(q1) = T1(q1) ·(T1ref)-1·T2ref

8. After rotating joint 2 q2 T2R T0 q1
T2(q1,q2) = T1(q1) ·(T1ref)-1·T2ref·R(q2)

9. After rotating joint 2 q2 T2R T0 q1
Denote T2->1ref = (T1ref)-1·T2ref (frame relative to parent)
T2(q1,q2) = T1(q1) ·T2->1ref·R(q2)

10. General Formula
Denote 𝑇 𝑖→𝑖−1 𝑟𝑒𝑓 = 𝑇 𝑖−1 𝑟𝑒𝑓 −1 𝑇 𝑖 𝑟𝑒𝑓 (ref frame relative to parent)
𝑇 𝑘 𝑞 1 ,…, 𝑞 𝑘 = 𝑖=1 𝑘 𝑇 𝑖→𝑖−1 𝑟𝑒𝑓 𝑅( 𝑞 𝑖 )
T2(q1,q2)
T3(q1,..,q3) L2
T4(q1,…,q4) T0 L3
T1(q1) L1 L0

11. Generalization to tree structures
Topological sort: p[k] = parent of link k
Denote 𝑇 𝑖→𝑝[𝑖] 𝑟𝑒𝑓 = 𝑇 𝑝 𝑖 𝑟𝑒𝑓 −1 𝑇 𝑖 𝑟𝑒𝑓 (frame i relative to parent)
Let A(i) be the list of ancestors of i (sorted from root to i)
𝑇 𝑘 𝑞 1 ,…, 𝑞 𝑘 = 𝑖∈𝐴(𝑘) 𝑇 𝑖→𝑝[𝑖] 𝑟𝑒𝑓 𝑅( 𝑞 𝑖 )

12. To 3D…
Much the same, except joint axis must be defined (relative to parent)
Angle-axis parameterization

13. Generalizations Prismatic joints Ball joints Cylindrical joints
Spirals
Free-floating bases
From LaValle, Planning Algorithms

14. Configuration Space
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
q=(q1,…,qn) q1 q2 q3 qn

15. Rigid Robot 3-parameter representation: q = (x,y,q)
workspace
robot
reference direction q y
reference point x
3-parameter representation: q = (x,y,q)
In a 3-D workspace q would be of the form (x,y,z,a,b,g)

16. Articulated Robot q1 q2
q = (q1,q2,…,q10)

17. Protein

18. Configuration Space q y x Space of all its possible configurations
But the topology of this space is in general not that of a Cartesian space q q’
3-D cylinder embedded in 4-D space y q
robot x x y q 2p S1
R2S1

19. Configuration Space C = S1 x S1
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

20. Configuration Space C = S1xS1 Space of all its possible configurations
But the topology of this space is in general not that of a Cartesian space
C = S1xS1

21. Configuration Space C = S1xS1 Space of all its possible configurations
But the topology of this space is in general not that of a Cartesian space
C = S1xS1

22. 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

23. What is its topology? q1 q2
(S1)7xI3
(I: Interval of reals)

24. Notion of a (Geometric) Pathq 1 3 n 4 2
t(s)
A path in C is a piece of continuous curve connecting two configurations q and q’: t : s  [0,1]  t (s)  C

25. 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

27. Notion of Trajectory vs. Pathq 1 3 n 4 2
t(t)
A trajectory is a path parameterized by time: t : t  [0,T]  t (t)  C

28. Translating & Rotating Rigid Robot in 2-D Workspacex y q
reference point
robot
reference direction
workspace q x y 2p
configuration space
What is the placement of the robot in the workspace at configuration (0,0,0)?

29. Translating & Rotating Rigid Robot in 2-D Workspacex y q
reference point
robot
reference direction
workspace q x y 2p
configuration space
What is the placement of the robot in the workspace at configuration (0,0,0)? 29

30. Translating & Rotating Rigid Robot in 2-D Workspacex y q
reference point
robot
reference direction
workspace q 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

31. 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. A RobotModel stores a current configuration
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() gets the current transformation of the link (last set by setConfig)
3D rigid transform utilities in se3.py