CPSC 452 Inverse Kinematics Prof. Oussama Khatib, Stanford

CPSC 452 Inverse Kinematics Prof. Oussama Khatib, Stanford University Prof. Dezhen Song, Texas A&M University

X Workspace

Reachable Workspace Dexterous Workspace X

Given X Find q=(1, 2, 3)

Algebraic Solution The kinematics of the example seen before are: c123 s

123 B 0

T T

W 3 0

0 s123 0 l1c1 l2 c12

c123 0 l1s1 l2 s12 0 1

0 0 0

1 Assume goal point is specified by 3 numbers:

c s B

WT 0

0 s c

0 0 0 x

0 y 1 0 0 1

Algebraic Solution (cont.) By comparison, we get the four equations: c c123

s s123 x l1c1 l2 c12

y l1s1 l2 s12 Summing the square of the last 2 equations: x 2 y 2 l12 l22 2l1l2 c2

From here we get an expression for c2 2

2 2 1

x y l l c2 2l1l2

2 2

Algebraic Solution (III) When does a solution exist? What is the physical meaning if no solution exists? Two solutions for 2 are possible. Why?

Using c12=c1c2-s1s2 and s12= c1s2-c2s1: x k1c1 k 2 s1 y k1s1 k 2 c1

where k1=l1+l2c2 and k2=l2s2. To solve these eqs, set r=+ k12+k22 and =Atan2(k2,k1). Algebraic Solution (IV)

k1

l2 2

k2 l1

Then: k1=r cos , k1=r sin , and we can write: x/r= cos cos 1 - sin sin 1 y/r= cos cos 1 - sin sin 1 or: cos(+1) = x/r, sin(+1) =y/r

Algebraic Solution (IV) Therefore: +1 = Atan2(y/r,x/r) = Atan2(y,x)

and so: 1 = Atan2(y,x) - Atan2(k2,k1) Finally, 3 can be solved from: 1+ 2+ 3 =

Geometric Solution IDEA: Decompose spatial geometry into several plane geometry problems

y L2 L1 x

Applying the law of cosines: x2+y2=l12+l22 - 2l1l2cos(180+2)