lecture10

lecture10 - 역역역역역 역역역 역역 Implementation...

Info iconThis preview shows page 1. Sign up to view the full content.

View Full Document Right Arrow Icon
This is the end of the preview. Sign up to access the rest of the document.

Unformatted text preview: 역역역역역 역역역 역역 Implementation of Inverse Kinematics and Application 역역역역역 역역역역역 역역역역역역역역역역 역역역 kjchoi@graphics.snu.ac.kr Content What is Inverse Kinematics? Redundancy Basic Method Issues NLP-based method Jacobian-based 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 Ultra-super redundant How to solve highly redundant system? Content What is Inverse Kinematics? Redundancy Basic Method Issues NLP-based method Jacobian-based 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 NLP-based Method Inverse Kinematics problem as non-linear 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 Jacobian-based 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 y-roll 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 NLP-based method Jacobian-based method Resolving Redundancy Multiple Goals Application : Motion Retargetting Differential Kinematics x = f (θ) = Jθ x ∂f J≡ ∂θ = J −1x θ J: Jacobian Matrix Linearly relates end-effector 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 )) Open-loop vs. Closed-loop 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 NLP-based method Jacobian-based 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 NLP-based method Jacobian-based 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 Jacobian-based method Same formulation No weighting (t ) = J + x(t ) θ Content What is Inverse Kinematics? Redundancy Basic Method Issues NLP-based method Jacobian-based method Resolving Redundancy Multiple Goals Application : Motion Retargetting Summary Inverse Kinematics Solver Issues NLP-based Solver Jacobian-based Solver Resolving Redundancy Multiple Goals Motion Retargetting Thank You ...
View Full Document

Ask a homework question - tutors are online