Inverse Kinematics. Mike Bailey.

Transcription

1 Inverse Kinematics This work is licensed under a Creative Commons Attribution-NonCommercial- NoDerivatives 4.0 International License Mike Bailey mjb@cs.oregonstate.edu inversekinematics.pptx

2 Inverse Kinematics Forward Kinematics solves the problem if I know the link transformation parameters, where are the links?. Inverse Kinematics (IK) solves the problem If I know where I want the links to be (X,Y), what link transformation parameters will put them there? (X,Y) Ground

3 Inverse Kinematics (IK): Things Need to Move What Parameters Will Make Them Do That?

4 Remember the Idea Behind Newton s Method? Pick an initial guess of the input parameter and keep refining it until the answer it produces is close enough 1. y dy dx x. x y have dy dx 3. x x x Now we just need to do it with more than one input parameter

5 Jacobian Method Our input parameters (in this case) are the three rotation angles. If (X, Y) are where we want the end of the third link to end up, then: X f(,, ) 1 3 and Y If you have a good guess for ( 1,, 3), which currently produce an (X, Y) that is not quite what you want, then you can refine your values for ( 1,, 3) and try again. The fundamental equations for this are: g(,, ) 1 3 X X approx X X 1 1 X X 3 3 Y Y approx Y Y 1 1 Y Y 3 3

6 Y Y Y X X X Or, in matrix form: is called the Jacobian, and is abbreviated as [ J ]: Y X Y Y Y X X X The matrix: Y X J 3 1 x 3 3 x 1 x 1 The Jacobian Matrix

7 Solving the Equations Note that [J] is not a square matrix, so this system of equations cannot be solved for directly. But, if we pre-multiply by the transpose of [ J ], we get: [ J ] T J 1 3 [ J ] T X Y ( 3 x x 3 ) 3 x 1 ( 3 x x 1 ) 3 x 3 3 x 1 3 x 1 which is solvable because it is 3-equations-3-unknowns. It is not obvious, but this is the Least Squares formulation. It will give an optimum (ΔΘ 1, ΔΘ,ΔΘ 3 ) to make (X,Y) move closer to the desired values.

8 Iterating to get a Solution (note similarity to Newton s Method) Differentiate the equations X f(,, ) and Pick a starting Θ1, Θ, and Θ3 1. Compute X approx and Y approx from 1 3 ( 1,, 3). Compute ΔX=X-X approx and ΔY=Y-Y approx 3. If ΔX and ΔY are small enough, we re done Y g(,, ) Compute: X 1, X, X 3, Y 1, Y, Y 3 5. Form the Jacobian [ J ] 6. Solve the system of equations: [ J ] T J 1 [ J ] 3 T X Y 7. Refine: ; ;

9 The Calculus Product Rule Tells us How to Differentiate Scalar Variables that have been Multiplied c ab dc d( ab) da db ba d d d d

10 [ M] [ T][ R ] The Calculus Product Rule Tells us How to Differentiate Matrices that have been Multiplied dm [ ] d([ T][ R ]) dt [ ] d[ R ] [ R ] [ T] d d d d 0 dr [ ] d cos sin 0 0 sin cos 0 0 d sin cos 0 0 cos sin 0 0 d

11 Numerical Differentiation Rather then explicitly differentiating an equation, it is often easier to calculate the derivative numerically. This is known as the Central Difference method, where you look both forward and backwards to see how the dependent variable is changing with respect to the independent variable: X X( ) X( ) Pick a delta that is small, but not so small that floating point accuracy becomes an issue

12 Numerical Differentiation If there are other independent variables, hold them constant: X X( 11,, 3) X( 11,, 3) 1 1

13 Numerical Differentiation In case you ever need it, here is how to compute the second derivative X X X X ( ) ( ) ( ) As before, if there are other independent variables, hold them constant:

14 Another IK Approach: Cyclic Coordinate Descent (CCD) Method The idea is to change Θ1 so that (X,Y) are as close to (X,Y) as possible. Then change Θ. Then change Θ3. Then change Θ1. Then change Θ. Then change Θ3. Then change Θ (X,Y) 1 Ground (X,Y)

15 Changing Θ1 Holding Θ and Θ3 constant, rotate Θ1 so that the blue lines line up. 3 (X,Y) 1 Ground (X,Y)

16 Changing Θ1 (X,Y) Ground (X,Y)

17 Changing Θ Holding Θ1 and Θ3 constant, rotate Θ so that the blue lines line up. (X,Y) Ground (X,Y)

18 Changing Θ (X,Y) Ground (X,Y)

19 Changing Θ3 Holding Θ1 and Θ constant, rotate Θ3 so that the blue lines line up. (X,Y) Ground (X,Y)

20 Changing Θ3 (X,Y) Ground (X,Y)

21 Now, do it again -- Changing Θ1 (X,Y) Ground (X,Y)

22 Now, do it again -- Changing Θ1 (X,Y) (X,Y)

23 Now, do it again -- Changing Θ (X,Y) (X,Y)

24 Now, do it again -- Changing Θ (X,Y) (X,Y)

25 Now, do it again -- Changing Θ3 (X,Y) (X,Y)

26 Now, do it again -- Changing Θ3 (X,Y) (X,Y)

27 Computing how much to change a rotation (in this example, we are changing θ ) Where we want to be: (X,Y ) Where we are now: (X 3,Y 3 ) Y Ground X Use the C/C++ atan( ) function: = atan( Y -Y, X -X ); = atan( Y 3 -Y, X 3 -X ); Δ = - Do not use the C/C++ atan( ) function: = atan( (Y -Y ) / (X -X ) ); = atan( (Y 3 -Y ) / (X 3 -X ) ); Δ = -