Transcript Inverse Kinematics How do I put my hand here? IK: Choose these angles!

Inverse Kinematics
How do I put my
hand here?
IK: Choose these angles!
Example: Planar 3-link robot
x  l1 cos 1  l2 cos1   2   l3 cos1   2   3 
y  l1 sin 1  l2 sin 1   2   l3 sin 1   2   3 
  1   2   3
3
2
What is the reachable space? Take l1, l2
fixed and vary 3
l3
l2
l1
Take l1  l2  l3 , l1  l2  l3
1
Now vary 1
Finally, vary
2
The Workspace
Workspace
• Workspace: volume of space which can be
reached by the end effector
• Dextrous workspace: volume of space where the
end effector can be arbitrarily oriented
• Reachable workspace: volume of space which
the robot can reach in at least one orientation
Example (continued)
What is the dextrous workspace in the example?
The IK Problem
• Kinematic Problem: given joint angles and/or
displacement, compute location and orientation of
End Effector.
• Inverse Kinematic Problem: given location and
orientation of EE, find joint variables.
• Why is IK hard?
– May have more than one solution or none at all
– Amounts to solving nonlinear trascendental
equations (can be hard)
Existence of Solutions
• A solution to the IKP exists if the target belongs
to the workspace
• Workspace computation may be hard. In practice
is made easy by special design of the robot
• The IKP may have more than one solution. How
to choose the appropriate one?
2 solutions!
Methods of Solutions
• A manipulator is solvable if the joint variables
can be determined by an algorithm. The
algorithm should find all possible solutions.
closed form solutions
•
numerical solutions
Solutions
We are interested in closed-form solutions
1. Algebraic Methods
2. Geometric Methods
Method of Solution (cont.)
• Major result: all systems with revolute and
prismatic joints having a total of six degrees of
freedom in a single series chain are solvable
• In general, solution is numerical
• Robots with analytic solution: several intersecting
joint axes and/or many i = 0, 90o.
• One major application (and driving force) of IK:
animation.
Manipulator Subspace when n<6
• If n<6, then the workspace will be a portion of an
n dimensional subspace
• To describe the WS: compute direct kinematics,
and then vary joint variables
• On the previous example, the WS has the form:
c
s

B

T

W
0

0
 s
c
0
0
0 x

0 y
1 0

0 1
Manipulator SS when n<6 (cont)
• Usual goal for manipulator with n DoF: use n
parameters to specify the goal
• If 6 DoF are used, n<6 will in general not suffice
• Possible compromise: reach the goal as “near” as
possible to original goal:
S
G
– 1) Given the goal frame T compute modified goal
S
S
in
manipulator
SS
as
near
as
possible
to
T
GT
G'
– 2) Compute IK. A solution may still not be possible if
goal is not in the manipulator workspace
• For example, place tool frame origin at desired
location, then select a feasible orientation
Algebraic Solution
The kinematics of the example seen before are:
c123  s123
s
c
123
123
B
0

T

T

W
3
 0
0

0
 0
0 l1c1  l2 c12 
0 l1s1  l2 s12 

1
0

0
1

Assume goal point is specified by 3
numbers:
c  s 0 x 
s

B

WT 
0

0
c
0
0
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  l 2  l 2  2l l c
1
2
1 2 2
From here we get an expression for c2
x  y l l
c2 
2l1l2
2
2
2
1
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  sin 1 + sin  cos 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
2
y
L2
L1
x
Applying the “law of cosines”:
x2+y2=l12+l22  2l1l2cos(1802)
Geometric Solution (II)
Then:
x  y l l
c2 
2l1l2
2
2
2
1
The LoC gives:
2
2
y


l22 = x2+y2+l12 - 2l1 (x2+y2) cos 
x
So that cos  = (x2+y2+l12 - l22 )/2l1 (x2+y2)
We can solve for 0   180, and then
1=
Reduction to Polynomial
• Trascendental equations difficult to solve since
one variable  usually appears as cos  and sin.
• Can reduce to polynomial in variable
u = tan /2
by using:
cos  = (1-u2)/(1+u2)
sin  =2u /(1+u2)
How? Use the fact that sin  =2[sin(/2)cos (/2)]
and cos  =[cos(/2)2- sin(/2)2]
Piper’s Solution – 3 axis intersect
• Solution for manipulators with 6DOF’s when
three consecutive axis intersect
• We will consider the case of revolute joints and
last three axis intersect
• Recall the transformation:
 s i
0
ai 1 
 c i
 s c

c

c


s


s

d
i i 1
i i 1
i 1
i 1 i 
i 1

T

i
 s i s i 1 c i s i 1 c i 1
c i 1di 


0
0
0
1


Piper’s Solution
0
P4ORG 01T 21T 23T 3P4ORG  x
y z 1T
The vector P4ORG in the 3-frame has the form:
 a3 
  s d 
3 4
3
P4ORG  
 c 3d 4 


 1 
And in the 3-frame:
a3c3  d 4 s 3s3  a2
 a3   f1  3  

 s d   f    a c s  d s c c  d s c  d s 
3 4  2 3   3 2 3
4 3 2 3
4 2 3
3 2
2
P4ORG  23T 


 c 3d 4   f3  3  a3s 2 s3  d 4 s 3s 2c3  d 4c 2c 3  d3c 2 

 
 

1
1
1

 
 

Piper’s Solution (II)
Repeating patiently
c1g1  s1g 2 
s g  c g 
0
P4ORG   1 1 1 2 


g3


1


g1  c2 f1  s2 f 2  a1
g 2  s2c1 f1  c2c1 f 2  s1 f3  d 2 s1
g3  s2 s1 f1  c2 s1 f 2  c1 f3  d 2c1
As mentioned before, for rotational joint
does not depend on 1:
r
0
P4ORG
2
 x 2  y 2  z 2  g12  g 22  g32
0
P4ORG
Replace the gi’s, using z = g3 and work patiently:
r  k1c2  k 2 s2 2a1  k3
z  k1s2  k 2c2 s1  k 4
k1  f1
k2   f 2
k3 
k4 
f12  f 22  f 32  a12  d 22  2d 2 f 3
f 3c1  d 2c1
1. If a1=0, r = k3(3). Solve for 3
2. If sin(1)=0, z = k4(3). Solve for 3
3. Otherwise, eliminate s2 and c2 above to get
r  k3 2  z  k4 2
4a12
s12
 k12  k 22
Piper’s Solution (IV)
• 1 and 2 above give a quadratic equation in
tan(3/2)
• 3 gives an equation of degree four
• Having solve 3, can solve above for 2 and 1
• The remaining angles can be computed to give
the desired orientation.
Example: PUMA560
• Want to solve:
 r11 r12 r13 p x 
r

r
r
p
21
22
22
y
6

T

0
 r31 r32 r33 p z 


0
0
1
0
01T (1 ) 21T ( 2 ) 23T ( 3 )34T ( 4 ) 45T ( 5 )56T ( 6 )
• TRICK: Invert transformations to separate vbles:


1 6
0
1
2
3
4
5
T
(

)
T

T
(

)
T
(

)
T
(

)
T
(

)
1
1
0
2
2 3
3 4
4 5
5 6T ( 6 )
PUMA560: DK Solution
• Then:
 c1 s1 0 0  r11
  s c 0 0  r
 1 1
  21
 0
0 1 0  r31


0 0 1  0
 0
61T ( 2  6 )
r12
r13
r22
r22
r32
r33
0
0
px 
p y 
pz 

1
• Equating (2,4) element from two sides:
 s1 px  c1 p y  d3
• Equation can also be obtained from “geometrical”
arguments
• Two possible solutions
The size of the translation in {1} is independent of 1:
p x2  p 2y  p z2  a22  a32  d32  d 42
a3c3  d 4 s3 
K
2 a2
• Geometrical meaning?
• Two possible solutions
• Write:


1 6
0
1
2
3
4
5
T
(

)
T
(

)
T
(

)
T

T
(

)
T
(

)
1
1 2
2 3
3
0
4
4 5
5 6T ( 6 )
• Repeat now a similar procedure