Unformatted text preview: 역역역역역 역역역 역역 Implementation of Inverse
Kinematics and Application
역역역역역
역역역역역
역역역역역역역역역역
역역역
[email protected] Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting What is Inverse Kinematics? Forward Kinematics
? θ1
Base θ2 θ3 x = f(θ) End Effector What is Inverse Kinematics? Inverse Kinematics θ1
Base θ2 θ3 −1 θ = f ( x) End Effector What does θ2 l1 θ1
Base looks like?
f(θ) l2 ? l3 End Effector θ3 x = l1 cos(θ1 ) + l2 cos(θ 2 ) + l3 cos(θ 3 )
y = l1 sin(θ1 ) + l2 sin(θ 2 ) + l3 sin(θ 3 ) Solution to −1 θ = f ( x) Our example
x = l1 cos(θ1 ) + l2 cos(θ 2 ) + l3 cos(θ 3 )
y = l1 sin(θ1 ) + l2 sin(θ 2 ) + l3 sin(θ 3 ) Number of equation : 2
Unknown variables : 3
Infinite number of solutions ! Redundancy System DOF > End Effector DOF Our example
x = l1 cos(θ1 ) + l2 cos(θ 2 ) + l3 cos(θ 3 )
y = l1 sin(θ1 ) + l2 sin(θ 2 ) + l3 sin(θ 3 )
System DOF = 3
End Effector DOF = 2 Redundancy A redundant system has infinite number of
solutions Human skeleton has 70 DOF Ultrasuper redundant How to solve highly redundant system? Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting What is NLP? Non Linear Programming
Method to optimize a nonlinear function Example minimize x 2 ( y + 1) + sin( x + y )
subject to x ≥ 0, y ≥ 0 Objective function
Constraint
Iterative algorithm NLPbased Method Inverse Kinematics problem as nonlinear
optimization problem Minimization of Goal Potential Function Zhao and Badler, 1994, ACMTOG Goal Potential Function “Distance” from the end effector to the goal Function of joint angles : G(θ ) Our Example
Goal
distance θ1
Base θ2 θ3 End Effector Goal Potential Function
Position Goal pg − pe 2 Orientation Goal
e2
x r −r
g
x e2
y + r −r
g
y Position/Orientation Goal
2 e2
x ω p g − p e + (1 − ω )c( r − r
g
x e2
y + r −r
g
y ) Our Example
x = l1 cosθ1 + l2 cosθ 2 + l3 cosθ 3
y = l1 sin θ1 + l2 sin θ 2 + l3 sin θ 3 • Goal Potential Function
G (θ) = ( x g − x) 2 + ( y g − y ) 2
= ( x g − (l1 cosθ1 + l2 cosθ 2 + l3 cosθ 3 )) 2
+ ( y g − (l1 sin θ1 + l2 sin θ 2 + l3 sin θ 3 )) 2 Nonlinear Optimization Recasted Constrained Optimization Problem minimize G (θ) T
a θ = b1 subject to T
a θ ≤ b 2 Nonlinear Optimization Available NLP Packages LANCELOT
DONLP2
MATLAB
Etc... Quiz Will G(θ ) be always zero? Will the solution be always found? No : Unreachable Workspace
No : Local Minima/Singular Configuration Will the solution be always unique? No : Redundancy Handling Singularity Singular Configuration Causes infinite joint velocity Occurs when any x cannot achieve given
θ Example Fully stretched limbs Remedy For Jacobianbased method Damped pseudo inverse
+ J = J (JJ + λ I) T T Dexterity measure
Clamping 2 −1 δ max δ min For nonlinear optimization method Try again with another initial value Parametric Singularity Gimbal Lock in Euler angle representation When a degree of freedom is lost, the gimbals is said to
“lock”
Consider a yroll of 90 degrees which aligns the x and z
axis 0
0
− 1 sin(θ − θ ) cos(θ − θ ) 0 R (θ x ,90,θ z ) = x
z
x
z cos(θ x − θ z ) sin(θ x − θ z ) 0 Remedy : Quaternion Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting Differential Kinematics x = f (θ) = Jθ
x ∂f
J≡
∂θ = J −1x θ J: Jacobian Matrix Linearly relates endeffector change to joint angle
change Differential Kinematics Our Example x f1 (θ) l1 cosθ1 + l2 cosθ 2 + l3 cosθ y = f (θ) = l sin θ + l sin θ + l sin θ 2 1
1
2
2
3
3 ∂f1 (θ) ∂θ
1
J= ∂f 2 (θ) ∂θ1 ∂f1 (θ)
∂θ 2
∂f 2 (θ)
∂θ 2 ∂f1 (θ) ∂θ 3 − l1 sin θ1
=
∂f 2 (θ) l1 cosθ1
∂θ 3 θ1 x y = J θ 2 θ3 − l2 sin θ
l2 cosθ − l3 sin θ 3 l3 cos θ Differential Kinematics Is J always invertible? No! Remedy : Pseudo Inverse
+ T −1 J = J (JJ ) = J + x (minimal norm solution) θ
T = J + x + (I −J + J ) φ (general solution) θ Null space = J + x + (I −J + J ) φ (general solution) θ The null space of J is the set of vectors
which have no influence on the constraints θ ∈ nullspace( J ) ⇔ Jθ = 0 The pseudoinverse provides an operator
which projects any vector to the null space of J J∆θ = ∆x
∆θ = J + ∆x + ( I − J + J ) z Utility of Null Space The null space can be used to reach
secondary goals
+
+
∆θ = J ∆x + ( I − J J ) z
min f (θ )
z Or to find comfortable positions f (θ ) = ∑ (θ comfort (i ) − θ (i ))
i 2 Null Space = (J + J − I )z
θ V = Jθ
+ V = J (J J − I )z
+ V = ( JJ J − J ) z
V = (J − J )z
V = 0⋅ z
V =0 Calculating Pseudo Inverse Gaussian Elimination
+ T −1 J = J (JJ ) T Singular Value Decomposition J = USV
+ T −1 J = VS U T How Can We Get θ(t )
From ?x(t ) (t ) = J + (t )x(t ) θ(t ) = ∫ θ
∫
t t Integrating Problems (t ) = J + x(t ) θ Initial tracking error
Numerical drift Remedy Error feedback (t ) = J + (x(t ) + Ke(t )) θ
e(t ) = x(t ) − f (θ(t )) Discretization (t ) = J + (x(t ) + Ke(t )) θ = J + (x + Ke )
n
θn
n
θ n − θ n −1
+ x n − x n −1
=J (
+ Ke n )
h
h
θ n − θ n −1 = J + (x n − f (θ n −1 )) Openloop vs. Closedloop scheme xn
f (θ n ) xn x n −1 e n −1 f (θ n )
f (θ n −1 ) Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting Redundancy Is Evil Multiple choices for one goal What happens if we pick any of them? Redundancy Is Good We can exploit redundancy Additional objective Minimal Change
Similarity to Given Example
Naturalness Minimal Change Pseudo Inverse Solution Minimal velocity norm solution x = Jθ → θ = J + x Minimal acceleration norm solution + Jθ → = J + ( − Jθ) = Jθ x
θ
x Penalty Term in Goal Potential Function θ
Gnew (θ) = G (θ) + θT M P θ + θT MV θ + T M A
θ Similarity to Given Example Adding homogeneous solution term to the
pseudo inverse solution = J + x + (I −J + J )K (θ − θ) θ
e New Goal Potential Function Gnew (θ) = G (θ) + k θ e − θ 2 Naturalness Based on observation of natural human posture
Neurophysiological experiments
Example Pointing task with pen stylus
Linear mapping between shoulder joint and pen stylus
in spherical coordinate system Analytic solution for human arm endgame.mov Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting Multiple Goals Conflict Between Goals ee 2 ee 1
base Conflict Between Goals Goal 1 ee 2 ee 1
base Conflict Between Goals Goal 2 ee 2 ee 1
base Conflict Between Goals Goal 2
Goal 1 ee 2 ee 1
base Conflict Between Goals Goal 2
Goal 1 ee 2 ee 1
base Handling Multiple Goals Weighted sum of each goal potential function
m G (θ) = ∑ ω i Gi (θ)
all i =1 Jacobianbased method Same formulation
No weighting (t ) = J + x(t ) θ Content What is Inverse Kinematics?
Redundancy
Basic Method Issues NLPbased method
Jacobianbased method
Resolving Redundancy
Multiple Goals Application : Motion Retargetting Summary Inverse Kinematics
Solver Issues NLPbased Solver
Jacobianbased Solver
Resolving Redundancy
Multiple Goals Motion Retargetting Thank You ...
View
Full
Document
This note was uploaded on 01/23/2012 for the course CS 551 taught by Professor Staff during the Fall '10 term at Syracuse.
 Fall '10
 Staff

Click to edit the document details