Inverse Kinematics (part 2) CSE169: Computer Animation Instructor: Steve Rotenberg UCSD, Spring 2016

Size: px

Start display at page:

Download "Inverse Kinematics (part 2) CSE169: Computer Animation Instructor: Steve Rotenberg UCSD, Spring 2016"

Dwain Waters
5 years ago
Views:

1 Inverse Knematcs (part 2) CSE169: Computer Anmaton Instructor: Steve Rotenberg UCSD, Sprng 2016

2 Forward Knematcs We wll use the vector: Φ M to represent the array of M jont DOF values We wll also use the vector: e e e e N to represent an array of N DOFs that descrbe the end effector n world space. For example, f our end effector s a full jont wth orentaton, e would contan 6 DOFs: 3 translatons and 3 rotatons. If we were only concerned wth the end effector poston, e would just contan the 3 translatons.

3 Forward & Inverse Knematcs The forward knematc functon computes the world space end effector DOFs from the jont DOFs: e f Φ The goal of nverse knematcs s to compute the vector of jont DOFs that wll cause the end effector to reach some desred goal state Φ f 1 e

4 Gradent Descent We want to fnd the value of x that causes f(x) to equal some goal value g We wll start at some value x 0 and keep takng small steps: x +1 = x + Δx untl we fnd a value x N that satsfes f(x N )=g For each step, we try to choose a value of Δx that wll brng us closer to our goal We can use the dervatve to approxmate the functon nearby, and use ths nformaton to move downhll towards the goal

5 Gradent Descent for f(x)=g df/dx f(x ) f-axs g x +1 x x-axs

6 Gradent Descent Algorthm } at new // evaluate step along // take 1 slope // compute { whle at // evaluate value startng ntal x f x f f x s f g x x x dx df s g f x f x f f x

7 Jacoban Inverse Knematcs

8 Jacobans N M M N x f x f x f x f x f x f x f d d J , x f x f

9 Jacobans Let s say we have a smple 2D robot arm wth two 1-DOF rotatonal jonts: e=[e x e y ] φ 2 φ 1

10 Jacobans The Jacoban matrx J(e,Φ) shows how each component of e vares wth respect to each jont angle , y y x x e e e e J Φ e

11 Jacobans Consder what would happen f we ncreased φ 1 by a small amount. What would happen to e? e e x e y T e φ 1

12 Jacobans What f we ncreased φ 2 by a small amount? e e x e y T e φ 2

13 Jacoban for a 2D Robot Arm φ 2 φ , y y x x e e e e J Φ e e

14 Incremental Change n Pose Lets say we have a vector ΔΦ that represents a small change n jont DOF values We can approxmate what the resultng change n e would be: de e Φ J, dφ e Φ Φ J Φ

15 Incremental Change n Effector What f we wanted to move the end effector by a small amount Δe. What small change ΔΦ wll acheve ths? e Φ so : J Φ J 1 e

16 Incremental Change n e Gven some desred ncremental change n end effector confguraton Δe, we can compute an approprate ncremental change n jont DOFs ΔΦ Δe Φ J 1 e φ 2 φ 1

17 Incremental Changes Remember that forward knematcs s a nonlnear functon (as t nvolves sn s and cos s of the nput varables) Ths mples that we can only use the Jacoban as an approxmaton that s vald near the current confguraton Therefore, we must repeat the process of computng a Jacoban and then takng a small step towards the goal untl we get to where we want to be

18 Choosng Δe We want to choose a value for Δe that wll move e closer to g. A reasonable place to start s wth Δe = g - e We would hope then, that the correspondng value of ΔΦ would brng the end effector exactly to the goal Unfortunately, the nonlnearty prevents ths from happenng, but t should get us closer Also, for safety, we wll take smaller steps: Δe = β(g - e) where 0< β 1

19 Basc Jacoban IK Technque whle (e s too far from g) { Compute J(e,Φ) for the current pose Φ Compute J -1 // nvert the Jacoban matrx } Δe = β(g - e) // pck approxmate step to take ΔΦ = J -1 Δe Φ = Φ + ΔΦ // apply change to DOFs Compute new e vector // apply forward // knematcs to see // where we ended up // compute change n jont DOFs

20 Invertng the Jacoban Matrx

21 Invertng the Jacoban If the Jacoban s square (number of jont DOFs equals the number of DOFs n the end effector), then we mght be able to nvert the matrx Most lkely, t won t be square, and even f t s, t s defntely possble that t wll be sngular and non-nvertable Even f t s nvertable, as the pose vector changes, the propertes of the matrx wll change and may become sngular or near-sngular n certan confguratons The bottom lne s that just relyng on nvertng the matrx s not gong to work

22 Underconstraned Systems If the system has more degrees of freedom n the jonts than n the end effector, then t s lkely that there wll be a contnuum of redundant solutons (.e., an nfnte number of solutons) In ths stuaton, t s sad to be underconstraned or redundant These should stll be solvable, and mght not even be too hard to fnd a soluton, but t may be trcky to fnd a best soluton

23 Overconstraned Systems If there are more degrees of freedom n the end effector than n the jonts, then the system s sad to be overconstraned, and t s lkely that there wll not be any possble soluton In these stuatons, we mght stll want to get as close as possble However, n practce, overconstraned systems are not as common, as they are not a very useful way to buld an anmal or robot (they mght stll show up n some specal cases though)

24 Well-Constraned Systems If the number of DOFs n the end effector equals the number of DOFs n the jonts, the system could be well constraned and nvertable In practce, ths wll requre the jonts to be arranged n a way so ther axes are not redundant Ths property may vary as the pose changes, and even well-constraned systems may have trouble

25 Pseudo-Inverse If we have a non-square matrx arsng from an overconstraned or underconstraned system, we can try usng the pseudonverse: J*=(J T J) -1 J T Ths s a method for fndng a matrx that effectvely nverts a non-square matrx Some propertes of the pseudonverse: J*J=I JJ*=I (J*)*=J and for square matrces, J*=J -1

26 Degenerate Cases Occasonally, we wll get nto a confguraton that suffers from degeneracy If the dervatve vectors lne up, they lose ther lnear ndependence

27 Sngle Value Decomposton The SVD s an algorthm that decomposes a matrx nto a form whose propertes can be analyzed easly It allows us to dentfy when the matrx s sngular, near sngular, or well formed It also tells us about what regons of the multdmensonal space are not adequately covered n the sngular or near sngular confguratons The bottom lne s that t s a more sophstcated, but expensve technque that can be useful both for analyzng the matrx and nvertng t

28 Jacoban Transpose Another technque s to smply take the transpose of the Jacoban matrx! Surprsngly, ths technque actually works pretty well It s much faster than computng the nverse or pseudo-nverse Also, t has the effect of localzng the computatons. To compute Δφ for jont, we compute the column n the Jacoban matrx J as before, and then just use: Δφ = J T Δe

29 Jacoban Transpose Wth the Jacoban transpose (JT) method, we can just loop through each DOF and compute the change to that DOF drectly Wth the nverse (JI) or pseudo-nverse (JP) methods, we must frst loop through the DOFs, compute and store the Jacoban, nvert (or pseudo-nvert) t, then compute the change n DOFs, and then apply the change The JT method s far frendler on memory access & cachng, as well as computatons However, f one prefers qualty over performance, the JP method mght be better

30 Iteratng to the Soluton

31 Iteraton Whether we use the JI, JP, or JT method, we must address the ssue of teraton towards the soluton We should consder how to choose an approprate step sze β and how to decde when the teraton should stop

32 When to Stop There are three man stoppng condtons we should account for Fndng a successful soluton (or close enough) Gettng stuck n a condton where we can t mprove (local mnmum) Takng too long (for nteractve systems) All three of these are farly easy to dentfy by montorng the progress of Φ These rules are just coded nto the whle() statement for the controllng loop

33 Fndng a Successful Soluton We really just want to get close enough wthn some tolerance If we re not n a bg hurry, we can just terate untl we get wthn some floatng pont error range Alternately, we could choose to stop when we get wthn some tolerance measurable n pxels For example, we could poston an end effector to 0.1 pxel accuracy Ths gves us a scheme that should look good and automatcally adapt to spend more tme when we are lookng at the end effector up close (level-of-detal)

34 Local Mnma If we get stuck n a local mnmum, we have several optons Don t worry about t and just accept t as the best we can do Swtch to a dfferent algorthm (CCD ) Randomze the pose vector slghtly (or a lot) and try agan Send an error to whatever s controllng the end effector and tell t to try somethng else Bascally, there are few optons that are truly appealng, as they are lkely to cause ether an error n the soluton or a possble dscontnuty n the moton

35 Takng Too Long In a tme crtcal stuaton, we mght just lmt the teraton to a maxmum number of steps Alternately, we could use nternal tmers to lmt t to an actual tme n seconds

36 Iteraton Step Sze We want to lmt the step sze we take by scalng t by some value β where 0< β 1 As β s just a scalar, we can actually choose t after computng ΔΦ A reasonable approach s to lmt the steppng of the jont rotatons so that we never rotate more than some threshold value (maybe 5 degrees) n a sngle teraton To do ths, we compute ΔΦ wthout β, then check to see f any values of ΔΦ exceed our threshold β = threshold / max(threshold, max(abs(δφ )))

37 Iteraton Step Sze Δe = g - e ΔΦ = J -1 Δe β = threshold / max(threshold, max(abs(δφ ))) Φ = Φ + β(δφ) Note that ths works for any IK method that nteratvely generates ΔΦ (JI, JT, JP, etc.)

38 Other IK Issues

39 Jont Lmts A smple and reasonably effectve way to handle jont lmts s to smply clamp the pose vector as a fnal step n each teraton One can t compute a proper dervatve at the lmts, as the functon s effectvely dscontnuous at the boundary The dervatve gong towards the lmt wll be 0, but comng away from the lmt wll be non-zero. Ths leads to an nequalty condton, whch can t be handled n a contnuous manner We could just choose whether to set the dervatve to 0 or non-zero based on a reasonable guess as to whch way the jont would go. Ths s easy n the JT method, but can potentally cause trouble n JI or JP

40 Hgher Order Approxmaton The frst dervatve gves us a lnear approxmaton to the functon We can also take hgher order dervatves and construct hgher order approxmatons to the functon Ths s analogous to approxmatng a functon wth a Taylor seres

41 Repeatablty If a gven goal vector g always generates the same pose vector Φ, then the system s sad to be repeatable Ths s not lkely to be the case for redundant systems unless we specfcally try to enforce t If we always compute the new pose by startng from the last pose, the system wll probably not be repeatable If, however, we always reset t to a comfortable default pose, then the soluton should be repeatable One potental problem wth ths approach however s that t may ntroduce sharp dscontnutes n the soluton

42 Multple End Effectors Remember, that the Jacoban matrx relates each DOF n the skeleton to each scalar value n the e vector The components of the matrx are based on quanttes that are all expressed n world space, and the matrx tself does not contan any actual nformaton about the connectvty of the skeleton Therefore, we extend the IK approach to handle tree structures and multple end effectors wthout much dffculty We smply add more DOFs to the end effector vector to represent the other quanttes that we want to constran However, the ssue of scalng the dervatves becomes more mportant as more jonts are consdered

43 Multple Chans Another approach to handlng tree structures and multple end effectors s to smply treat t as several ndvdual chans Ths works for characters often, as we can anmate the body wth a forward knematc approach, and then anmate each lmb wth IK by postonng the hand/foot as the end effector goal Ths can be faster and smpler, and actually offer a ncer way to control the character

44 Geometrc Constrants One can also add more abstract geometrc constrants to the system Constran dstances, angles wthn the skeleton Prevent bones from ntersectng each other or the envronment Apply dfferent weghts to the constrants to sgnfy ther mportance Have addtonal controls that try to maxmze the comfort of a soluton Etc. Welman talks about ths n secton 5

45 Other IK Technques Cyclc Coordnate Descent Ths technque s more of a trgonometrc approach and s more heurstc. It does, however, tend to converge n fewer teratons than the Jacoban methods, even though each teraton s a bt more expensve. Welman talks about ths method n secton 4.2 Analytcal Methods For smple chans, one can drectly nvert the forward knematc equatons to obtan an exact soluton. Ths method can be very fast, very predctable, and precsely controllable. Wth some fnesse, one can even formulate good analytcal solvers for more complex chans wth multple DOFs and redundancy Other Numercal Methods There are lots of other general purpose numercal methods for solvng problems that can be cast nto f(x)=g format

46 Jacoban Method as a Black Box The Jacoban methods were not nvented for solvng IK. They are a far more general purpose technque for solvng systems of non-lnear equatons The Jacoban solver tself s a black box that s desgned to solve systems that can be expressed as f(x)=g ( e(φ)=g ) All we need s a method of evaluatng f and J for a gven value of x to plug t nto the solver If we desgn t ths way, we could concevably swap n dfferent numercal solvers (JI, JP, JT, damped least-squares, conjugate gradent )

47 Computng the Jacoban

48 Computng the Jacoban Matrx We can take a geometrc approach to computng the Jacoban matrx Rather than look at t n 2D, let s just go straght to 3D Let s say we are just concerned wth the end effector poston for now. Therefore, e s just a 3D vector representng the end effector poston n world space. Ths also mples that the Jacoban wll be an 3xN matrx where N s the number of DOFs For each jont DOF, we analyze how e would change f the DOF changed

49 1-DOF Rotatonal Jonts We wll frst consder DOFs that represents a rotaton around a sngle axs (1-DOF hnge jont) We want to know how the world space poston e wll change f we rotate around the axs. Therefore, we wll need to fnd the axs and the pvot pont n world space Let s say φ represents a rotatonal DOF of a jont. We also have the offset r of that jont relatve to t s parent and we have the rotaton axs a relatve to the parent as well We can fnd the world space offset and axs by transformng them by ther parent jont s world matrx

50 1-DOF Rotatonal Jonts To fnd the pvot pont and axs n world space: a r W W parent parent a r Remember these transform as homogeneous vectors. r transforms as a poston [r x r y r z 1] and a transforms as a drecton [a x a y a z 0]

51 Rotatonal DOFs Now that we have the axs and pvot pont of the jont n world space, we can use them to fnd how e would change f we rotated around that axs e a e r Ths gves us a column n the Jacoban matrx

52 Rotatonal DOFs e e e a e r e r a : unt length rotaton axs n world space r : poston of jont pvot n world space e: end effector poston n world space r a

53 3-DOF Rotatonal Jonts For a 2-DOF or 3-DOF jont, we just have 2 or 3 columns n the Jacoban matrx, one for each separate rotaton Remember that we want to do all of the computatons n world space- however t s actually a lttle trcky to get the world space rotaton axes Consder how we would fnd the world space x-axs of a 3-DOF ball jont Not only do we need to consder the parent s world matrx, but we need to nclude the rotaton around the next two axes (y and z-axs) as well Ths s because those followng rotatons wll rotate the frst axs tself

54 3-DOF Rotatonal Jonts For example, assumng we have a 3-DOF ball jont that rotates n XYZ order: Where R y (θ y ) and R z (θ z ) are y and z rotaton matrces parent z z parent y y z z parent W a R W a R R W a : : : dof z dof y dof x T T T

55 3-DOF Rotatonal Jonts Remember that a 3-DOF XYZ ball jont s local matrx wll look somethng lke ths: Where R x (θ x ), R y (θ y ), and R z (θ z ) are x, y, and z rotaton matrces, and T(r) s a translaton by the (constant) jont offset So t s world matrx looks lke ths: x x y y z z z y x R R R r T L,, x x y y z z parent R R R r T W W

56 3-DOF Rotatonal Jonts Once we have each axs n world space, each one wll get a column n the Jacoban matrx At ths pont, t s essentally handled as three 1-DOF jonts, so we can use the same formula for computng the dervatve as we dd earler: e a e r We repeat ths for each of the three axes

57 Quaternon Jonts What about a quaternon jont? How do we ncorporate them nto our IK formulaton? We wll assume that a quaternon jont s capable of rotatng around any axs However, snce we are tryng to fnd a way to move e towards g, we should pck the best possble axs for achevng ths a e r g r e r g r

58 Quaternon Jonts r g r e r g r e a e r g r e r a g

59 Quaternon Jonts We compute a drectly n world space, so we don t need to transform t Now that we have a, we can just compute the dervatve the same way we would do wth any other rotatonal axs e a e r We must remember what axs we use, so that later, when we ve computed Δφ, we know how to update the quaternon

60 Translatonal DOFs For translatonal DOFs, we start n the same way, namely by fndng the translaton axs n world space If we had a prsmatc jont (1-DOF translaton) that could translate along an arbtrary axs a defned n the parent s space, we can use:

61 Translatonal DOFs For a more general 3-DOF translatonal jont that just translates along the local x, y, and z-axes, we don t need to do the same thng that we dd for rotaton The reason s that for translatons, a change n one axs doesn t affect the other axes at all, so we can just use the same formula and plug n the x, y, and z axes [ ], [ ], [ ] to get the 3 world space axes Note: ths wll just return the a, b, and c axes of the parent s world space matrx, and so we don t actually have to compute them!

62 Translatonal DOFs As wth rotaton, each translatonal DOF s stll treated separately and gets ts own column n the Jacoban matrx A change n the DOF value results n a smple translaton along the world space axs, makng the computaton trval: e a

63 Translatonal DOFs e a

64 Buldng the Jacoban To buld the entre Jacoban matrx, we just loop through each DOF and compute a correspondng column n the matrx If we wanted, we could use more elaborate jont types (scalng, translaton along a path, shearng ) and stll compute an approprate dervatve If absolutely necessary, we could always resort to computng a numercal approxmaton to the dervatve

65 Unts & Scalng What about unts? Rotatonal DOFs use radans and translatonal DOFs use meters (or some other measure of dstance) How can we combne ther dervatves nto the same matrx? Well, t s really a bt of a hack, but we just combne them anyway If desred, we can scale any column to adjust how much the IK wll favor usng that DOF

66 Unts & Scalng For example, we could scale all rotatons by some constant that causes the IK to behave how we would lke Also, we could use ths as an addtonal way to get control over the behavor of the IK We can store an addtonal parameter for each DOF that defnes how stff t should behave If we scale the dervatve larger (but preserve drecton), the soluton wll compensate wth a smaller value for Δφ, therefore makng t act stff There are several proposed methods for automatcally settng the stffness to a reasonable default value. They generally work based on some functon of the length of the actual bone. The Welman paper talks about ths.

67 End Effector Orentaton

68 End Effector Orentaton We ve examned how to form the columns of a Jacoban matrx for a poston end effector wth 3 DOFs How do we ncorporate orentaton of the end effector? We wll add more DOFs to the end effector vector e Whch method should we use to represent the orentaton? (Euler angles? Quaternons? ) Actually, a popular method s to use the 3 DOF scaled axs representaton!

69 Scaled Rotaton Axs We learned that any orentaton can be represented as a sngle rotaton around some axs Therefore, we can store an orentaton as an 3D vector The drecton of the vector s the rotaton axs The length of the vector s the angle to rotate n radans Ths method has some propertes that work well wth the Jacoban approach Contnuous and consstent No redundancy or extra constrants It s also a nce method to store ncremental changes n rotaton

70 6-DOF End Effector If we are concerned about both the poston and orentaton of the end effector, then our e vector should contan 6 numbers But remember, we don t actually need the e vector, we really just need the Δe vector To generate Δe, we compare the current end effector poston/orentaton (matrx E) to the goal poston/orentaton (matrx G) The frst 3 components of Δe represent the desred change n poston: β(g.d - E.d) The next 3 represent a desred change n orentaton, whch we wll express as a scaled axs vector

71 Desred Change n Orentaton We want to choose a rotaton axs that rotates E n to G We can compute ths usng some quaternons: M=E -1 G q.frommatrx(m); Ths gves us a quaternon that represents a rotaton from E to G To extract out the rotaton axs and angle, we just remember that: q cos 2 a sn 2 sn 2 We can then scale the fnal axs by β x a y a z sn 2

72 End Effector So we now can defne our goal wth a matrx and come up wth some desred change n end effector values that wll brng us closer to that goal: e t x t y t z We must now compute a Nx6 Jacoban matrx, where each column represents how a partcular DOF wll affect both the poston and orentaton of the end effector x y z T

73 Rotatonal DOFs We need to compute addtonal dervatves that show how the end effector orentaton changes wth respect to an ncremental change n each DOF We wll use the scaled axs to represent the ncremental change For a rotatonal DOF, we frst fnd the rotaton axs n world space (as we dd earler) Then- we re done! That axs already represents the ncremental rotaton caused by that DOF By default, the length of the axs should be 1, ndcatng that a change of 1 n the DOF value results n a rotaton of 1 radan around the axs. We can scale ths by a stffness value f desred

74 Rotatonal DOFs The column n the Nx6 Jacoban matrx correspondng to a rotatonal DOF s: a s the rotaton axs n world space r s the pvot pont n world space e pos s the poston of the end effector n world space T T pos a r e a e J

75 Translatonal DOFs Translatonal DOFs don t affect the end effector orentaton, so ther contrbuton to the dervatve of orentaton wll be [0 0 0] T a e J

Inverse Kinematics (part 1) CSE169: Computer Animation Instructor: Steve Rotenberg UCSD, Winter 2018

Inverse Kinematics (part 1) CSE169: Computer Animation Instructor: Steve Rotenberg UCSD, Winter 2018 Welman, 1993 Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation, Chris