681 – Introduction to Computer Graphics
Rick Parent – CIS682
Inverse Kinematics
Set goal configuration of end effector
calculate interior joint angles
Analytic approach – when linkage is simple enough, directly calculate joint angles in configuration that satifies goal
Numeric approach – complex linkages
At each time slice, determine joint movements that take you in direction of goal position (and orientation)
Rick Parent – CIS682
Forward Kinematics – review
Pose – linkage is a specific configuration
Pose Vector – vector of joint angles for linkage
Degrees of Freedom (DoF) – of joint or of whole figure
Articulated linkage – hierarchy of joint-link pairs
Types of joints: revolute, prismatic
Tree structure – arcs & nodes
Recursive traversal – concatenate arc matrices
Push current matrix leaving node downward
Pop current matrix traversing back up to node
Rick Parent – CIS682
Goal
End Effector
q1
q2
q3
L1
L2
L3
Inverse Kinematics
Rick Parent – CIS682
Underconstrained – if fewer constraints than DoFs
Many solutions
Overconstrained – too many constraints
No solution
Dextrous workspace – volume end effector can reach in any orientation
Reachable workspace – volume the end effector can reach
Inverse Kinematics
Rick Parent – CIS682
Given arm configuration (L1, L2, …)
Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, y1,y2, y3]
Analytically compute goal configuration (q1,q2)
Interpolate pose vector from initial to goal
Inverse Kinematics – Analytic
Rick Parent – CIS682
Analytic Inverse Kinematics
(X,Y)
L1
L2
q1
q2
Goal
Rick Parent – CIS682
Analytic Inverse Kinematics
(X,Y)
Goal
Multiple solutions
Rick Parent – CIS682
Analytic Inverse Kinematics
(X,Y)
L1
L2
q1
qT
180- q2
Rick Parent – CIS682
Analytic Inverse Kinematics
(X,Y)
L1
L2
q1
qT
180- q2
X
Y
Rick Parent – CIS682
Law of Cosines
A
B
C
a
Rick Parent – CIS682
Analytic Inverse Kinematics
(X,Y)
L1
L2
q1
qT
180- q2
X
Y
Rick Parent – CIS682
Analytic Inverse Kinematics
Rick Parent – CIS682
Iterative Inverse Kinematics
When linkage is too complex for analytic methods
At each time step, determine changes to joint angles that take the end effector toward goal position and orientation
Need to recompute at each time step
Rick Parent – CIS682
End Effector
q2
a2
d2=EF-J2
a2 x d2
– Compute instantaneous effect of each joint
– Linear approximation to curvilinear motion
– Find linear combination to take end effector towards goal position
Inverse Jacobian Method
Rick Parent – CIS682
Inverse Jacobian Method
Instantaneous linear change in end effector for ith joint
= (EF – Ji) x ai
Rick Parent – CIS682
Inverse Jacobian Method
What is the change in orientation of end effector induced by joint i that has axis of rotation a i
and position Ji?
Angular velocity
Rick Parent – CIS682
Solution only valid for an
instantaneous step
Angular affect is really
curved, not straight line
Once a step is taken, need
to recompute solution
Inverse Jacobian Method
Rick Parent – CIS682
Inverse Jacobian Method
– Mathematics
Set up equations
yi: state variable
xi : system parameter
fi : relate system parameters to state variable
Rick Parent – CIS682
Inverse Jacobian Method
– Mathematics
Matrix Form
Rick Parent – CIS682
Inverse Jacobian Method
– Mathematics
Use chain rule to differentiate equations to relate changes in system parameters to changes in state variables
Rick Parent – CIS682
Matrix Form
Inverse Jacobian Method
– Mathematics
Rick Parent – CIS682
Inverse Jacobian Method
Change in position (and orientation) of end effector
Change in joint angles
Linear approximation that relates change in joint angle to change in end effector position (and orientation)
Rick Parent – CIS682
Inverse Jacobian Method
Rick Parent – CIS682
Inverse Jacobian Method
Rick Parent – CIS682
Inverse Jacobian Method
= (S – J1) x a1
= w1
Rick Parent – CIS682
The Matrices
Rick Parent – CIS682
The Matrices
V – desired linear and angular velocities
J – Jacobian
Matrix of partials
q – change to joint angles (unknowns)
3×1, 6×1
3xN, 6xN
N DoFs
N x 1
Rick Parent – CIS682
Pseudo Inverse of the Jacobian
Rick Parent – CIS682
Solving using the Pseudo Inverse
LU decomposition
Rick Parent – CIS682
Adding a Control Term
But it can be used to bias
The solution vector
A solution of this form
…doesn’t affect the desired configuration
When put into this formula
Like this
After some manipulation, you can show that it…
Rick Parent – CIS682
Form of the Control Term
Bias to desired angles
(not the same as hard joint limits)
Where the deviation is large, you bump up the solution vector in such a way that you don’t disturb the desired effect
Desired angles and corresponding gains are input
‘z’ is H differentiated
Rick Parent – CIS682
Some Algebraic Manipulation
Include this in equation
Isolate vector of unknown
Rearrange to isolate the inverse
Rick Parent – CIS682
Solving the Equations
LU decomp.
Rick Parent – CIS682
Use to bias to desired mid-angle
Does not enforce joint angles
Does not address “human-like” or “natural” motion
Control Term
Only kinematic control – no forces involved
Rick Parent – CIS682
Jacobian transpose
Alternate Jacobian – use goal position
HAL – human arm linkage
Other ways to numerically IK
CCD
Damped Least Squares
Rick Parent – CIS682
Jacobian Transpose
Use projection of effect vector onto desired movement
Rick Parent – CIS682
Jacobian Transpose
S
Rick Parent – CIS682
Jacobian Transpose
…
Rick Parent – CIS682
Alternate Jacobian
…
G
Use the goal postion instead of the end-effector!!??
!?
Rick Parent – CIS682
Damped Least Squares
G
substitution
Solve
Rick Parent – CIS682
Hueristic Human-Like Linkage (HAL)
3 DoF
3 DoF
1 DoF
G
Decompose into simpler subproblems
Fix wrist position – use as Goal
7 DoF linkage
RA (q1,q2 ,q3) , RB ( q4), RC(q5,q6 ,q7)
Set hand position and rotation based on relative position of Goal to shoulder
Rick Parent – CIS682
Hueristic Human-Like Linkage (HAL)
Set q4 based on distance between shoulder and wrist
s
e
w
Assume axis of elbow is perpendicular to plane defined by s, e, w use law of cosines
L1
L2
L
Rick Parent – CIS682
Hueristic Human-Like Linkage (HAL)
Determine elbow position based on heuristics
s
w
e
For example:
project forearm straight from hand orientation
Clamp to inside of limits
if arm intersects torso or a shoulder angle exceeds joint limit (or exceeds comfort zone) –
Elbow lies on circle defined by w, s & q4
q4
Rick Parent – CIS682
Hueristic Human-Like Linkage (HAL)
From e and w and hand orientation, determine RB
s
w
e
From e and s, determine RA
q4
Rick Parent – CIS682
Traverse linkage from distal joint inwards
Optimally set one joint at a time
Update end effector with each joint change
Cyclic-Coordinate Descent
Use weighted average of position and orientation.
At each joint, minimize difference between end effector and goal
Easy if only trying to match position; heuristic if orientation too
Rick Parent – CIS682
Cyclic-Coordinate Descent
.
Rick Parent – CIS682
Cyclic-Coordinate Descent
Rotational joint:
.
Rick Parent – CIS682
Cyclic-Coordinate Descent
Rotational joint:
.
Rick Parent – CIS682
Cyclic-Coordinate Descent
Rotational joint:
.
Rick Parent – CIS682
Cyclic-Coordinate Descent
Rotational joint:
.
Rick Parent – CIS682
Cyclic-Coordinate Descent
Translational joint:
..
Rick Parent – CIS682
IK w/ constraints
Chris Welman, “Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001.
Basic idea:
Constraints are geometric, e.g., point-to-point, point-to-plan, specific orientation, etc.
Assume starting out in satisfied configuration
Forces are applied to system
Detect, and cancel out, force components that would violate constraints.
Rick Parent – CIS682
IK w/ constraints
Point-on-a-plane constraint
Fa
Fc
Ft
Given: geometric constraints & applied forces
Determine: what constraints will be violated & what (minimal) forces are needed to counteract the components of the applied forces responsible for the violations.
Rick Parent – CIS682
Constraints
.
To maintain constraints, need:
Notation:
Rick Parent – CIS682
Constraints
.
Generalized force
Constraint Jacobian
IK Jacobian
Rick Parent – CIS682
example constraint
Usually sparse
.
Geometric constraint on point that is function of pose
Rick Parent – CIS682
Computing the constraint force
To counteract ga’s affect on constraints:
.
Applied force
Yet to be determined constraint force
g should lie in the nullspace of JcK
Rick Parent – CIS682
Computing the constraint force
.
Solve linear system to find Lagrange multiplier vector.
The system is usually underconstrained
Restrict gc to move the system in a direction it may not go
Rick Parent – CIS682
Solving for Lagrange Multipliers
Shortest distance from A to B passing through a point P
Constrain P to lie on g
g
A
B
Points on ellipse are set of points for which sum of distances to foci is equal to some constant
Direction of gradients are equal
gradient
Rick Parent – CIS682
Solving for Lagrange Multipliers
Use truncated SVD with backsubstitution on
diagonal
Range basis
Nullspace basis.
Rick Parent – CIS682
Solving for Lagrange Multipliers
Rick Parent – CIS682
Feedback term
Spring that penalizes deviation from constraints.
Rick Parent – CIS682
Implementation
Handles on skeletons
Point handle
orientation handle
Center-of-mass handle
Each handle must know how to compute the Jacobian.
Each handle must know how to its value from q
Rick Parent – CIS682
Constraints on handles
Constraining a point handle to a location
Constraining a point handle to a plane
Constraining a point handle to a line
Constraining an orientation handle to an orientation
.
Rick Parent – CIS682
Dataflow approach
..
Constraint function block
Knows how to compute
Its function in term of x
Knows its Jacobian wrt x
Rick Parent – CIS682
Example network
..
c1
Jc
C
h1
h2
h3
h4
q
c2
x
2
+
y
2
y
1
=
f
1
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
y
2
=
f
2
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
y
3
=
f
3
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
y
4
=
f
4
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
y
5
=
f
5
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
y
6
=
f
6
(
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
)
)
(
)
)
((
S
G
a
J
G
n
n
–
×
´
–
Y
=
F
(
X
)
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ë
é
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ë
é
=
ú
ú
ú
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ê
ê
ê
ë
é
n
z
y
x
z
n
y
y
y
n
x
x
x
z
y
x
z
y
x
a
a
a
p
p
p
p
p
p
p
v
v
v
q
q
q
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
w
w
w
&
K
&
&
K
K
K
K
K
K
K
K
K
K
K
K
K
K
2
1
1
1
1
1
2
1
2
1
6
6
5
5
4
4
3
3
2
2
1
1
dx
x
f
dx
x
f
dx
x
f
dx
x
f
dx
x
f
dx
x
f
dy
i
i
i
i
i
i
i
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
+
+
+
+
+
=
q
q
q
q
&
&
&
&
=
=
=
=
+
–
–
V
J
J
J
J
J
V
J
J
J
J
J
V
J
J
V
T
T
T
T
T
T
1
1
)
(
)
(
V
J
T
=
D
q
J
+
=
(
J
T
J
)
–
1
J
T
=
J
T
(
J
J
T
)
–
1
0
0
)
(
)
(
)
(
)
(
=
=
–
=
–
=
–
=
=
–
=
+
+
+
V
z
V
z
J
J
V
z
J
J
JJ
V
z
I
J
J
J
V
J
V
z
I
J
J
q
q
&
&
H
I
J
J
J
J
V
q
q
Ñ
–
–
=
+
)
(
&
H
=
a
i
i
=
1
n
å
(
q
i
–
q
ci
)
y
z
=
Ñ
q
H
=
dH
d
q
=
y
a
i
i
=
1
n
å
(
q
i
–
q
ci
)
y
–
1
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ë
é
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ê
ë
é
=
ú
ú
ú
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ê
ê
ê
ë
é
n
z
y
x
z
n
y
y
y
n
x
x
x
z
y
x
z
y
x
a
a
a
p
p
p
p
p
p
p
v
v
v
q
q
q
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
w
w
w
&
K
&
&
K
K
K
K
K
K
K
K
K
K
K
K
K
K
2
1
1
1
1
1
2
1
2
1
H
I
J
J
V
J
q
q
Ñ
–
+
=
+
+
)
(
&
H
H
J
V
JJ
J
H
H
J
V
JJ
J
H
H
J
V
J
H
H
J
J
V
J
H
I
J
J
V
J
T
T
T
T
q
q
q
q
q
q
q
q
q
q
q
q
q
q
Ñ
–
Ñ
+
=
Ñ
–
Ñ
+
=
Ñ
–
Ñ
+
=
Ñ
–
Ñ
+
=
Ñ
–
+
=
–
–
+
+
+
+
+
)]
(
)
[(
)
(
)
(
)
(
)
(
1
1
&
&
&
&
&
)
,
,
,
,
,
(
6
5
4
3
2
1
1
x
x
x
x
x
x
f
y
i
=
V
J
I
J
J
T
T
1
2
)
(
–
–
=
D
l
q
q
&
J
V
=
b
b
q
b
q
q
q
)
(
)
(
)
(
1
T
T
T
JJ
H
J
V
H
J
H
J
V
JJ
=
Ñ
+
Ñ
–
=
Ñ
+
=
–
&
q
&
J
V
=
AB
C
B
A
2
)
cos(
2
2
2
–
+
=
a
2
2
)
cos(
Y
X
X
T
+
=
q
2
2
1
2
2
2
2
2
1
1
2
)
cos(
Y
X
L
L
Y
X
L
T
+
–
+
+
=
–
q
q
T
Y
X
L
L
Y
X
L
q
q
+
+
–
+
+
=
–
)
2
(
cos
2
2
1
2
2
2
2
2
1
1
1
(
)
)
2
(
cos
180
2
1
2
2
2
2
2
1
1
2
L
L
Y
X
L
L
+
–
+
–
=
–
q
(
)
2
1
2
2
2
2
2
1
2
2
)
180
cos(
L
L
Y
X
L
L
+
–
+
=
–
q
÷
÷
ø
ö
ç
ç
è
æ
+
=
–
2
2
1
cos
Y
X
X
T
q
i
i
a
w
=
&
[
]
[
]
ú
ú
ú
ú
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ê
ê
ê
ê
ë
é
=
=
=
n
z
z
z
n
y
y
y
n
x
x
x
T
n
T
z
y
x
z
y
x
a
a
a
p
p
p
p
p
p
J
v
v
v
V
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
¶q
¶
q
q
q
q
q
w
w
w
…
…
…
…
…
…
…
…
2
1
2
1
2
1
3
2
1
&
&
&
&
&
q
&
J
V
=
6
6
5
5
4
4
3
3
2
2
1
1
dx
x
f
dx
x
f
dx
x
f
dx
x
f
dx
x
f
dx
x
f
y
i
i
i
i
i
i
i
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
¶
+
+
+
+
+
=
)
(
)
)
((
S
G
a
J
S
n
n
–
×
´
–
)
(
)
)
((
S
G
a
J
S
i
i
–
×
´
–
V
f
I
JJ
T
=
–
)
(
2
l
dX
X
F
dY
¶
¶
=
)
(
)
)
((
1
1
S
G
a
J
G
–
×
´
–
)
(
)
(
)
(
q
E
q
E
q
E
o
p
+
=
)
(
)
)
((
2
2
S
G
a
J
S
–
×
´
–
Alt
J
=
ú
ú
ú
û
ù
ê
ê
ê
ë
é
–
–
–
ú
ú
ú
ú
û
ù
ê
ê
ê
ê
ë
é
´
–
´
–
´
–
´
–
´
–
=
z
y
x
T
z
y
x
n
n
x
x
S
G
S
G
S
G
a
J
S
a
J
S
a
J
S
a
J
S
a
J
S
)
(
)
(
)
(
…
)
)
((
…
…
…
)
)
((
)
)
((
)
)
((
)
)
((
1
1
1
1
2
2
1
1
q
D
=
=
V
J
T
)
(
)
)
((
1
1
S
G
a
J
S
–
×
´
–
V
I
JJ
J
T
T
1
2
)
(
–
–
=
D
l
q
dX
X
F
dY
¶
¶
=
)
(
)
)
((
2
2
S
G
a
J
G
–
×
´
–
V
J
I
J
J
T
T
=
D
–
q
l
)
(
2
V
I
JJ
f
T
1
2
)
(
–
–
=
l
q
b
b
b
q
q
&
&
&
=
=
=
=
=
–
–
+
T
T
T
T
T
J
V
JJ
V
JJ
V
JJ
J
V
J
)
(
)
(
)
(
1
1
dX
X
F
dY
¶
¶
=
q
D
=
f
J
T
dX
X
F
dY
¶
¶
=
t
t
L
L
L
L
L
s
w
L
e
w
L
s
e
L
q
q
q
–
=
–
+
=
–
=
–
=
–
=
180
2
)
cos(
4
2
1
2
2
2
2
1
2
1
2
)
(
)
(
c
d
p
P
P
q
E
–
=
2
3
1
)
1
)
((
)
(
–
×
=
å
=
jc
j
jd
o
u
u
q
E
ic
axis
ic
P
R
P
i
)
(
)
(
f
f
=
¢
id
ic
p
P
P
g
×
¢
=
)
(
)
(
f
f
)
(
)
(
3
1
f
f
jc
j
jd
o
u
u
g
¢
×
=
å
=
)
(
)
(
)
(
f
f
f
o
o
p
p
g
w
g
w
g
+
=
p
f
+
c
j
k
k
i
j
i
c
q
P
P
c
q
c
J
¶
¶
¶
¶
=
¶
¶
=
)
(
)
(
)
(
f
f
f
o
o
p
p
g
w
g
w
g
+
=
p
f
–
c
)]
(
)
(
[
3
1
3
jc
j
jd
o
ic
id
p
i
u
u
w
P
P
w
axis
k
å
=
´
+
´
×
=
)
(
)
(
3
1
2
jc
j
jd
o
ic
id
p
u
u
w
P
P
w
k
å
=
×
+
×
=
1
=
o
w
)
1
(
r
a
+
=
p
w
W
k
/
=
a
)
,
max(
)
,
min(
ic
id
ic
id
P
P
P
P
=
r
)
sin(
)
cos(
))
cos(
1
(
)
(
3
2
1
f
f
f
f
k
k
k
g
+
+
–
=
)
(
)
(
)
)(
(
3
1
1
i
jc
j
i
jd
o
i
ic
i
id
p
axis
u
axis
u
w
axis
P
axis
P
w
k
×
×
+
×
×
=
å
=
0
)
cos(
)
sin(
)
(
3
2
1
=
+
–
f
f
k
k
k
)
)
(
(
tan
1
2
3
1
k
k
k
–
=
–
f
i
ic
id
axis
P
P
×
–
=
)
(
l
T
c
c
a
kCJ
g
g
K
q
–
+
=
)
(
&
f
i
i
i
w
q
q
+
=
Kg
J
C
c
=
&
j
i
c
q
c
J
¶
¶
=
0
)
(
=
¶
¶
=
q
q
C
q
C
&
&
b
A
=
l
a
c
c
c
Kg
J
Kg
J
–
=
0
)
(
=
q
C
0
))
(
(
2
=
–
q
P
R
[
]
]
)
(
,
)
(
,
)
[(
))
(
(
)),
(
(
)),
(
(
)
(
2
2
2
3
2
1
z
z
y
y
x
x
z
y
x
P
R
P
R
P
R
q
P
c
q
P
c
q
P
c
q
C
–
–
–
=
=
c
a
g
g
g
+
=
a
c
T
c
c
Kg
J
KJ
J
–
=
l
x
0
=
Kg
J
c
T
UDV
A
=
Kg
F
J
K
q
T
Þ
=
&
T
c
c
KJ
J
]
,…,
[
1
m
l
l
l
=
c
c
J
g
l
=
q
x
¶
¶
)
(
x
f
q
x
x
f
q
f
¶
¶
¶
¶
=
¶
¶
q
f
¶
¶
f
b
A
UDV
T
=
=
l
l
i
i
i
T
T
T
T
T
T
V
w
b
U
b
U
VD
b
U
D
V
b
U
DV
b
UDV
å
×
=
=
=
=
=
–
–
l
l
l
l
l
1
1
i
i
i
T
T
T
T
T
T
V
w
b
U
b
U
VD
b
U
D
V
b
U
DV
b
UDV
å
×
=
=
=
=
=
–
–
l
l
l
l
l
1
1