Advanced Algorithms. Kinematics. Kinematics. Reading. Hierarchical Kinematic Modeling. Rigid Body Constraints Controlling Groups of Objects

Transcription

1 Advanced Algorithms Kinematics CMPT 466 Computer Animation Torsten Möller Hierarchical Kinematic Modeling Forward Kinematics Inverse Kinematics Rigid Body Constraints Controlling Groups of Objects particle systems flocking behaviour Reading Chapter 4 of Parents book Foley, van Dam, etc.; Angel; Watt; Glassner; Hill; Baker + Hearns Kinematics Study of object movement Dynamics?

2 Hierarchical Kinematic Modeling Terminology Parent-child spatial relationships Joint Articulation of character Plants Planets, satellites, solar system, milky way Revolute Prismatic Links End effector Frame, Pose Joint Representation Joint Representation Single degree of freedom (DOF) Multiple Degrees of Freedom Revolute or prismatic joint gimbal lock Local axis of rotation Need to transform to global coordinates Enforce joint limits?! Use axis-angle or quaternion representation! " #

3 Joint Representation Degrees of Freedom (DOFs) Note: User interface representation may not be the same used for internal representation and operations! " # Six variables x, y, and z roll, pitch, and yaw DoFs Robot Arm? Six again -base, -shoulder, -elbow, -wrist More Complex Joints DOF joints Gimbal Spherical (doesn t possess singularity) DOF joints Universal

4 Link and Node Representation transformation Transformation data geometry How to Represent Articulators Joint-Link Hierarchies Define Link data so its point of rotation is at the origin Rotate the link Position it relative to its parent link in the hierarchy Put it through all the transformations of its parent link Transformations Tree Representation Arc Transformations NodePtr Joint Node Transformations DataPtr LinkPtr[ ] Link

5 Tree Traversal Articulated Figures T 0 T. T. T. T. M = I M = T0 M = T0*T. M = T0*T.*T. M = T0*T. M = T0 M = T0*T. M = T0*T.*T. Articulated Figure Representation Hierarchy Representation Model links as nodes of a tree All body frames are local (relative to parent) Transf. affecting root affect all children Transf. affecting any node affect all its children

6 Link and Node Representation Tree Traversal Transformation (fixed) Transformation (changeable) Transformation data geometry T0 T. T. R. R. T. T. R. R. M = I M = T0 M = T0*T.*R. M = T0*T.*R.*T.*R. M = T0*T.*R. M = T0 M = T0*T.*R. M = T0*T.*T.*R. Tree Traversal Multiple DoFs at Joints T0 T. T. R. R. T. T. R. R. How do you represent more than one degree of freedom at a joint? a) Zero length segments Between DoF joints b) Multiple DoF transformation at joint

7 Forward vs. Inverse Kinematics Forward Kinematics Compute configuration (pose) given individual DOF values Inverse Kinematics Compute individual DOF values that result in specified end effector position What is Inverse Kinematics? Forward Kinematics Base!!!? x = f (","," ) = f( e) End Effector What is Inverse Kinematics? What is f? Inverse Kinematics!!! End Effector Base l! l! l!? f (","," ) End Effector Base (","," ) = e = f # (x) x = l cos(! ) + l y = l sin(! ) + l cos(! ) + l sin(! ) + l cos(! ) sin(! )

8 Forward Kinematics Forward Kinematics Hierarchical model - joints and links Joints - rotational or prismatic Joints -,, or Degree of Freedom Links - displayable objects Pose - setting parameters for all joint DoFs Pose Vector - a complete set of joint parameters Specifying A Pose Forward Kinematics $ $ $ Traverse kinematic tree and propagate transformations downward Use stack Compose parent transformation with child s Pop stack when leaf is reached High DOF models are tedious to control this way

9 Joint Transformations Rotation representation: matrix? Euler angles? Quaternions? Impose joint limits on rotations? Need complex functions to model human figure limits Could have translational joints, e.g., telescoping legs Pose vector: vector of all joint angles [!!!! n ] Interpolate between poses gimbal lock possible if DoF joints Animate a Pose Interpolate pose vectors Forward Kinematics Define Joint-Link Hierarchy Define sequence of keyposes with corresponding times For given time t, use keyposes to interpolate pose at time t Traverse tree hierarchy using pose vector to supply angles value Use key-values With key-poses - entire pose vector is specified Better to allow independent keys for each articulation variable (or avar) Sometimes called track-based frames

10 Local Frames - Denavit and Hartenberg (DH) Standard method of describing relationship of one DOF to next Used extensively in robotics Used in some early animation systems Multiple DOF joints represented by zerolength parameters Denavit and Hartenberg Revolute joints! i+ = rotation of joint i+ wrt to its z-axis relative to x-axis of joint i Denavit and Hartenberg DH notation X-axis of joint i is the line segment perpendicular to both z axes of i and i+ % i link twist parameter

11 Screw Transformations! Relationship between i+ frame and i frame are a combination i th joint parameters i+ joint parameters Screw transformations Two (translation, rotation) pairs each relative to specific axis of i th and i+ frames Screw Transformations Offset (d i+ ) and angle (! i+ ) are translation and rotation of i+ joint relative to i th joint w.r.t. z i -axis Length (a i ) and twist (% i ) are translation and rotation w.r.t. x i -axis D and H transformations D & H Transformations

12 Example Simple manipulator Example Ball and Socket Joint Ball and Socket Model as revolute joints with zero-length links between them If all angles are set to 0, we are in gimbal lock situation (z-axes of two joints are collinear) Instead, initialize middle joint angle to 90 degrees The x-axis of last frame is same as x-axis of previous frame when joint angle is zero Forward Kinematics - review Articulated linkage hierarchy of joint-link pairs Pose linkage is a specific configuration Pose Vector vector of joint angles for linkage Degrees of Freedom (DoF) of joint or of whole figure

13 Forward Kinematics - review 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 Inverse Kinematics How do I put my fingers in my eyes? Use IK: Choose these angles! Inverse Kinematics Inverse Kinematics Set goal configuration of end effector and calculate joint angles Analytic approach simple linkage 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) L!! L L! Goal End Effector

14 Inverse Kinematics - Analytic Inverse Kinematics - Analytic L L 80-!!! x + y t X (X,Y) Y Given arm configuration (L, L, ) Given desired goal position (and orientation) of end effector: [x,y] or [x,y,z, ",", "] Analytically compute goal configuration (!,!) Interpolate pose vector from initial to goal Inverse Kinematics - Analytic Inverse Kinematics Underconstrained if fewer constraints than DoFs Many solutions Overconstrained too many constraints No solution Reachable workspace volume the end effector can reach Dextrous workspace volume end effector can reach in any orientation

15 Configuration Space Configuration Space The set of all possible positions (defined by kinematics) an object can attain Work Space vs. Configuration Space Work space The space in which the object exists Dimensionality R for most things, R for planar arms Configuration space The space that defines the possible object configurations Degrees of Freedom Analytical IK Given end effector position, compute required joint angles In simple case, analytic solution exists Use trig, geometry, and algebra to solve

16 Inverse Kinematics - Analytic Law of Cosines Law of Cosines cos( " ) = " = a cos( L cos( "!" ) =! L " = acos(! x cos(80!" ) =! t t t x x " = acos( x x + y x + y )! L L + y! L L L! ( x + y! L L L x! L! ( x + y ) ) + " t L x + y! L + y ) + y! L ) Inverse Kinematics - Analytic x + y = a + a " a a cos(# "$ ) cos$ = x + y " a " a a a for greater accuracy tan $ = " cos$ = a a " x " y + a + a + cos$ a a + x + y " a " a ( ) " x + y = a + a x + y ( ) ( ) " a " a a $ = ±tan " + a x + y ( ) ( ) ( ) " x + y ( ) " a " a ( ) Two solutions: elbow up & elbow down y 0! O 0 y a & y "! (x,y) a O! x 0 x O x Solution to e = f " (x) Redundancy Our example x = l cos(! ) + l y = l sin(! ) + l cos(! ) + l sin(! ) + l Number of equation : Unknown variables : cos(! ) sin(! ) System DOF > End Effector DOF Our example x = l cos(! ) + l y = l sin(! ) + l System DOF = End Effector DOF = cos(! ) + l sin(! ) + l cos(! ) sin(! ) Infinite number of solutions!

17 Redundancy What about? A redundant system has infinite number of solutions Human skeleton has 70 DOF Ultra-super redundant How to solve highly redundant system? Inverse Kinematics - Numeric 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 Inverse Kinematics - Numerically #! d # x d End Effector - Compute instantaneous effect of each joint - Linear approximation to curvilinear motion - Find linear combination to take end effector towards goal position

18 Inverse Kinematics Inverse Kinematics 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 Kinematics - Math Matrix Form y = f (x,x,x,x 4,x 5,x 6 ) y = f (x,x,x,x 4,x 5,x 6 ) y = f (x,x,x,x 4,x 5,x 6 ) y 4 = f 4 (x,x,x,x 4, x 5,x 6 ) y 5 = f 5 (x,x,x,x 4,x 5,x 6 ) y 6 = f 6 (x,x,x,x 4,x 5,x 6 ) Use chain rule Y = F(X) "Y = "F "X "X "y i = "f i "x "x + "f i "x "x + "f i "x "x + "f i "x 4 "x 4 + "f i "x 5 "x 5 + "f i "x 6 "x 6

19 The Matrices The Matrices V = [ v x v y v z " x " y " z ] T # = [ #... # n ] T % $v x $v x $v... x ( ' $# $# $# * ' n * ' $v y $v y $v J =... y * ' $# $# $# n * ' * ' $" z $" z $"... z * ' * & $# $# $# n ) V = J " V linear and angular velocities J Jacobian Matrix of partials! change to joint angles N DoFs x, 6x xn, 6xN N x V = J! V linear and angular velocities J Jacobian Matrix of partials! change to joint angles Pseudo Inverse of the Jacobian Solving The Pseudo Inverse V = J " J T V = J T J " (J T J) # J T V = (J T J) # J T J " J + V = " J + = (J T J) " J T = J T (J J T ) " J + V = " J T (JJ T ) # V = " $ = (JJ T ) # V (JJ T )$ = V J T $ = " LU decomposition

20 Adding a Control Term Form of the Control Term A solution of this form Doesn t affect the desired configuration But it can be used to bias The solution vector " = (J + J # I)z V = J" V = J(J + J # I)z V = (JJ + J # J)z V = (J # J)z V = 0z V = 0 Bias to desired angles (not the same as hard joint limits) z is H differentiated n H = #" i ($ i %$ ci ) & i= Desired angles and corresponding gains are input z = ' $ H = dh d$ =& # " i($ i %$ ci ) &% n i= Some Algebraic Manipulation Solving the Equations Add this, using previous form to solution indicated by pseudo-inverse of Jacobian Rearrange and solve " = J + V + (J + J # I)$ " H " = J + V + (J + J # I)$ " H " = J + V + J + J$ " H # $ " H " = J + (V + J$ " H) # $ " H " = J T (JJ T ) # (V + J$ " H) # $ " H " = J T [(JJ T ) # (V + J$ " H)] # $ " H " = (JJ T ) # (V + J$ % H) % = J T " # $ % H V + J$ % H = (JJ T )" LU decomp.

21 Control Term Use to bias to desired mid-angle Does not enforce joint angles Does not address human-like or natural motion Only kinematic control no forces involved