A Detailed Look into Forward and Inverse Kinematics

Transcription

1 A Detailed Look into Forward and Inverse Kinematics Kinematics = Study of movement, motion independent of the underlying forces that cause them September 19-26, 2016

2 Kinematics Preliminaries Preliminaries: On transformation matrices

3 Kinematics of Simple Systems Triangle translating and rotating in 2D

4 Kinematics of Simple Systems What are the degrees of freedom of this system? What is its configurational space? Triangle translating and rotating in 2D

5 Kinematics of Interesting Systems Serial linkage in a 2D workspace (obstacles in gray)

6 Kinematics of Complex Systems Serial linkage with many links (many dofs) serpentine robots protein backbone chains

7 Rigid-Body Transformation z Preserves Euclidean distances between points in a rigid body T T(x) T(y) y T(y) T(x) = y-x x y A rigid-body transformation consists of: rotation and translation x

8 Rigid-body Transformation in 2D y x

9 Rigid-body Transformation in 2D y x

10 Rigid-body Transformation in 2D y x

11 Rigid-body Transformation in 2D y x

12 Rigid-body Transformation in 2D y Why? Rotation matrix: j i cos -sin sin cos t y Translation component: t x x t x t y

13 Rigid-body Transformation in 2D y Rotation matrix: j i i 1 j 1 i 2 j 2 t y t x x Vector (i 1,i 2 ) is new unit vector, with what coordinates in old world frame? What about (j 1,j 2 )?

14 Rigid-body Transformation in 2D y b j i a b Rotation matrix: = i 1 j 1 i 2 j 2 a b t y b v a a t x x Transformation of a point?

15 Homogeneous Coordinate Matrix in 2D y y t y y x t x x x The rotation and translation can be combined together in a homogeneous coordinate matrix What is a homogeneous coordinate matrix? Is translation a linear transformation?

16 Homogeneous Coordinate Matrix in 2D y y i 1 j 1 t x i 2 j 2 t y t y y x t x x x T = (t,r) T(x) = t + Rx x cos -sin t x x t x + x cos - y sin y = sin cos t y y = t y + x sin + y cos

17 Rigid-body Transformations in 3D 2? 1

18 Homogeneous Coordinate Matrix in 3D z y z k x i j y R i 1 j 1 k 1 t x i 2 j 2 k 2 t y i 3 j 3 k 3 t z x with: i i i 2 3 = 1 i 1 j 1 + i 2 j 2 + i 3 j 3 = 0 det(r) = +1 R -1 = R T Why? Why? Why? Why?

19 Rotations around Axes Plus Translation in 3D z z y y ccw rotation Rotation by around y axis: cos 0 sin t x t y -sin 0 cos t z xx Rotation by around x axis: Rotation by around z axis: cos -sin 0 t x sin cos 0 t y t z t x 0 cos -sin t y 0 sin cos t z

20 Rotation Around Arbitrary Vector v in 3D R(v, ) =? Step 1. Translate v to origin to obtain vector k Step 2. Rotate around centered vector k Step 3. Translate back Step 3. v How does one rotate around a centered vector? Step 2. k Step 1.

21 Rotation Around Centered Vector k in 3D R(k, ) = k x k x v + c k x k y v - k z s k x k z v + k y s k x k y v + k z s k y k y v + c k y k z v - k x s k x k z v - k y s k y k z v + k x s k z k z v + c where: k = (k x k y k z ) T s = sin c = cos v = 1-cos k

22 Rotation Around Centered Vector k in 3D How is R(k, ) obtained? 1. Rotate k so that the rotation axis is aligned with one of the principle x, y, z coordinate axes 2. Perform rotation of object about coordinate axis 3. Perform inverse rotation of 1 Details at aph/modeling/mod_tran/3drota.htm

23 Homogeneous Coordinate Matrix in 3D z y (x,y,z) z (x,y,z ) k x i j y x i 1 j 1 k 1 t x x y i 2 j 2 k 2 t y y z = i 3 j 3 k 3 t z z x Composition of two transforms represented by matrices T 1 and T 2 : T 2 T 1 Which one is applied first?

24 A Serial Linkage Model

25 Rotations in the Serial Linkage Model Rotating around a i by angle affects positions of following joints a i+2, a i+3, and others down the chain Rotation is about arbitrary vector b i (rotational axis shown) by specified/desired angle

26 Joint rotation in the Serial Linkage Model Anchor: First joint placed at origin of coordinate system Link b i defined from joint a i to a i+1 Rotating around a i by angle affects positions of following joints a i+2, a i+3, and others down the chain

27 Rotating a Bond in the Serial Linkage Model R(b i, ) = Translate(a i ) * R(axis, ) * Translate(-a i )

28 Chaining Rotations in a Serial Linkage Two rotations need to be applied at the same time: one around joint 3 by 30 deg, another around joint 5 by 15 degrees. Joints between bonds 3 to 5 updated by: But the joints after bond 5 are updated by:

29 Drawbacks of Homogeneous Coordinate Matrix x i1 j1 k1 tx x y i2 j2 k2 ty y z = i3 j3 k3 tz z Accumulation of computing errors along a serial linkage and repeated computation

30 Drawbacks of Homogeneous Coordinate Matrix x i1 j1 k1 tx x y i2 j2 k2 ty y z = i3 j3 k3 tz z Rotation representation in rotation matrices is redundant Only 3 parameters are actually needed Why 3-parameters for representing rotations?

31 Drawbacks of Homogeneous Coordinate Matrix x i1 j1 k1 tx x y i2 j2 k2 ty y z = i3 j3 k3 tz z A rotation representation expresses the orientation of a rigid body (or coordinate frame) relative to a reference frame. Rotation representation in homogeneous coordinate matrix is the matrix consisting of new axes i,j,k in the rotated coordinate frame. Rotation matrix is often called the Direction Cosine Matrix (DCM), as the new axes can be described in terms of their coordinates relative to the reference axes (recall our derivation of the rotation in 2D).

32 Drawbacks of Homogeneous Coordinate Matrix x i1 j1 k1 tx x y i2 j2 k2 ty y z = i3 j3 k3 tz z A rotation representation expresses the orientation of a rigid body (or coordinate frame) relative to a reference frame. Euler s rotation theorem: (1) The displacement of a rigid body (or coordinate frame) with one point fixed is described by a rotation about some axis. (2) Such a rotation may be uniquely described by a minimum of 3 dofs. Rotation matrix has a total of 9 parameters that are not independent Orthonormality specifies 6 constraints (3 for normality, 3 for orthogonality) A total of 9-6 = 3 independent parameters represent the rotation

33 Goal: Less Redundant Rotation Representations Rotation representations: Rotation matrix, Euler angles, Axis-angle, Unit Quaternions Rotation representation in rotation matrices is redundant Euler angles are an example of non-redundant 3-parameters representations of rotations Non-redundant 3-parameter representations of rotations like Euler angles have many problems: No simple algebra: composing rotations is not straightforward Singularities: many points map to same point in another representations The unit quaternion is a less redundant rotation representation that uses four parameters

34 Representations of Rotations A brief summary of rotation representations: matrix_.28or_direction_cosine_matrix.2c_dcm.29

35 Unit Quaternion (for Rotations in 3D) Quaternion: p = (a, bi, cj, dk) - 4 parameters Extensions of complex numbers i 2 = j 2 = k 2 = jk = -1 ij = k; jk = i; ki = j i; ji = -k; kj = -i; ik = -j Convenient to describe them as scalar plus vector: p = a + v, or p = (a, v) where vector v = <b c d> Unit quaternion: p 2 = 1 a,b,c,d can be defined so that p represents rotation around unit vector by a certain angle

36 Unit Quaternion (for Rotations in 3D) Allows compact representation of rotation R(r, ) around vector r by angle R(r, ) = (cos /2, r 1 sin /2, r 2 sin /2, r 3 sin /2) = (cos /2, r sin /2) R(r, ) Same rotation can be encoded in two ways (cos /2, r sin /2) or (cos (, -r sin ( ) R(-r, 2 ) Space of unit quaternions: Unit 3-sphere in 4-D space with antipodal points identified

37 Operations on Quaternions P = p 0 + p (scalar part is p 0, vector part is p) Q = q 0 + q (different operations can be defined) Product PQ is more interesting - it can be represented as another quaternion R = r 0 + r = PQ where r 0 = p 0 q 0 p.q (. denotes inner product) and r = p 0 q + q 0 p + p q ( denotes outer product) Conjugate of P is another quaternion P * = p 0 - p

38 Rotation of a Vector u Using Unit Quaternions Vector u = (x,y,z) can be represented as a quaternion 0 + x We want to rotate u around unit centered vector n by angle Let rotation R(n, ) be represented by a quaternion P R(n, ) Let P* be the conjugate of P Rotation of x yields x : 0 + x = P R(n, ) (0 + x) P* R(n, )

39 Forward and Inverse Kinematics Some more examples: Forward Kinematics on manipulators February 17, 2014

40 FK for Two-Linkage Chain x = l 1 cos 1 + l 2 cos( ) y = l 1 sin 1 + l 2 sin( ) 2 l 1 l 2 (x,y) 1

41 FK for Two-Linkage Chain x = l 1 cos 1 + l 2 cos( ) y = l 1 sin 1 + l 2 sin( ) x = x 1 + x 2 x 1 = l 1 cos 1 x 2 = l 2 cos( ) = l 2 cos(- ) y 1 y 2 2 l 1 1 x 2 l 2 (x,y) = - 1 ( 2 - ) = -( ) - = x 2 = l 2 cos( ) 1 x 1 x 2 y = y 1 - y 2 y 1 = l 1 sin 1 y 2 = l 2 sin( ) = -l 2 sin(- ) = l 2 sin( ) y = l 1 sin 1 + l2 sin( 1 + 2)

42 FK for Two-Linkage Chain x = l 1 cos 1 + l 2 cos( ) y = l 1 sin 1 + l 2 sin( ) 2 l 1 l 2 (x,y) Another, easier way Think of local frames and Rigid-body transformations 1 Think of local frames and rigid-body transformations

43 FK for Two-Linkage Chain x = l 1 cos 1 + l 2 cos( ) y = l 1 sin 1 + l 2 sin( ) (x 1,y 1 ) 2 l 1 l 2 (x 2,y 2 ) Another, easier way Think of local frames and Rigid-body transformations 1 Think of local frames and rigid-body transformations (x 0,y 0 ) What is (x 2,y 2 ) in coordinate system at (x 1,y 1 ) What is (x 1,y 1 ) in coordinate system at (x 0,y 0 ) Chose x-axes on links

44 FK for Two-Linkage Chain cos 1 -sin 1 x 0 (x 1,y 1 ) 2 j 1 i 1 l 2 j 2 T 1 0 = sin 1 cos 1 y l 1 (x 2,y 2 ) j 0 (x 0,y 0 ) 1 i 0 i 2 cos -sin l 1 T 2 1 = sin cos What is (x 2,y 2 ) in coordinate system at (x 0,y 0 ) Just accumulate transformations: (x 2,y 2 ) = T 1 0 * T 2 1 * [l2 0] T

45 Linkage (Internal Coordinate) Model anchor joint T? T? Can express any frame relative to any frame Just accumulate transformations

46 Relative Position of Two Joints T i+2 k-1 T i+1 i+1 T k k joint i i joint k T ik = position of joint k in joint i T ik = T i i+1 T i+1 i+2 T k-1 k [where chain grows from i to k]

47 Optional Reading (youtube video explains in detail): Denavit-Hartenberg Model derivation based on J.J. Craig. Introduction to Robotics. Addison Wesley, reading, MA, Research article : Zhang, M. and Kavraki, L. E.. A New Method for Fast and Accurate Derivation of Molecular Conformations. Journal of Chemical Information and Computer Sciences, 42(1):64 70,

48 Forward and Inverse Kinematics Inverse Kinematics February 17, 2014

49 IK In Robotics Solve for the dofs in order to satisfy spatial constraints on end effectors Extra

50 IK In Robotics Extra

51 IK In Computer Graphics, Games, Virtual Reality Real-Time Joint Coupling of the Spine for Inverse Kinematics Raunhardt, Boulic JVRB 2008 Extra

52 IK In Computational Biology Filling gaps in structure determination by X-ray crystallography Lotan, Bedem, Latombe Extra

53 IK In Computational Biology Computing conformational ensembles of loops in proteins Shehu, Proteins 2006 Extra

54 Solving the IK Problem for Two-Linkage Chain l 1 l 2 (x,y) 2 = cos -1 x 2 + y 2 l 12 l 2 2 2d 1 d 2 1 = -x(l 2 sin 2 ) + y(l 1 + d 2 cos 2 ) y(l 2 sin 2 ) + x(l 1 + l 2 cos 2 ) Two solutions

55 Solving the IK Problem for Two-Linkage Chain 2 l 1 l 2 (x,y) 1 Two solutions

56 A More Complicated Example 2 d 2 d 3 (x,y, ) d Finite number of solutions d 3 d 2 ( 1, 2, 3 ) d 1

57 General Results for the IK Problem 6-joint chain in 3-D space: N DOF =0 Why? At most 16 distinct IK solutions q 6 Target pose for end effector q 2 q 3 q 4 q 5 q 1

58 General Results for the IK Problem 6-joint chain in 3-D space: N DOF =0 Why? At most 16 distinct IK solutions q 6 End effector target pose q 2 q 3 q 4 q 5 q 1 6-joint chain 6 dofs Target pose 3 translation and 3 orientation constraints So: 6 6 = 0 free dofs IK solutions can be enumerated

59 Analytical or Exact IK Methods Can solve only for 6 joints Write forward kinematics in the form of polynomial equations (use t = tan( /2) Solve

60 IK Methods/Solvers Computer Science Exact IK solvers [Manocha, Canny 94] [Manocha et al. 95] [Zhang, Kavraki 02] [Zhang, White, Wang, Goldman, Kavraki 04] Biology/Crystallography Exact IK solvers [Go, Scheraga 70] [Wedemeyer, Scheraga 99] [Coutsias et al. 04] Optimization IK solvers Optimization IK solvers [Wang, Chen 91] [Fine et al. 86] [Shenkin et al. 87] Cyclic Coordinate Descent: Applications for protein loops [Canutescu, Dunbrack 03] [Han, Amato 00] [Xie, Amato 03] [Cortes, Simeon, Laumond 02] [Cortes et al. 04] [Shehu et al ]

61 Basic Idea of Iterative IK Methods Can solve only for arbitrary number of joints 1. Compute error e = target pose current pose 2. Find changes to joint values that minimize e 2 3. Apply through forward kinematics 4. Repeat until e 2 is below a threshold or we run out of patience for more iterations

62 IK as an Optimization (Minimization) Problem Q = (q 1 q 2 q n ): n-vector of dofs = ( 1 2 n ): n-vector of values to dofs k end effectors with current poses denoted s 1 s k Target poses for end effectors: t 1 t k Two fundamental observations: s 1 s k depend on ( 1 2 n ) through forward kinematics function: written as: s = s( ) IK problem is to find values for 1 2 n so that t i = s i ( ) for all i

63 IK as an Optimization (Minimization) Problem There may be no closed-form solution to t i = s i ( ) Iterative methods approximate a good solution We want ds/dt s.t. s + ds/dt = t So, ds/dt is velocity and we want it to be e Using chain rule: ds( )/dt = s/ ( ) * d /dt Velocity in s related to speed in theta via partial derivatives

64 IK as an Optimization (Minimization) Problem Let Ji( ) = s/ ( ) -- J is called the Jacobian matrix Note that J can be viewed as a mxn matrix (m = 3k) Then: ds( )/dt = J( ) * d /dt s 1 ( 1 )/ 1 s 1 ( 2 )/ 2 s 1 ( n )/ n s 2 ( 1 )/ 1 s 2 ( 2 )/ 2 s 2 ( n )/ n s 6 ( 1 )/ 1 s 6 ( 2 )/ 2 s 6 ( n )/ n

65 IK as an Optimization (Minimization) Problem So: ds( )/dt = J( ) * d /dt J( ) = s/ ( ) leads to an iterative way of solving t i = s i ( ): Given current values for, s, t, compute J( ) Find an update d s.t. the change ds = J( ) d updates s to reach t In other words, find d s.t. 0 = e( + d ) = t s( + d ) = J( ) d Iterative methods fall in two categories: (all in one):find values d by which to update all angles (one at a time). find d i to increment i, update s, then continue to i+1

66 Computing the Jacobian Jacobian entries s/ ( ) are usually not hard to calculate For rotational joints (see Buss review for other types of dofs) s i / ( j ) = v j x (s i - p j ) where v j is unit vector along the rotational axis for j and p j is the position of the joint Intuition: s i - p j, v j form a plane perpendicular to circle followed by link i in rotation around v j (forms basis of cyclic coordinate descent method)

67 How to Compute Inverse of Jacobian We want inverse of J, not J itself, because we want to find d ds/dt = J( ) d J -1 ( ) ds/dt = J -1 ( ) J( ) d = d So, by finding J -1 ( ), we find d

68 Finding Inverse of Jacobian is Not Trival J is an 6 n matrix. Assume rank(j) = 6 Find d s.t. e = J( ) d would mean d = J -1 e May not have rank 6, which means inverse may not exist Transpose or pseudo inverse are often used for J -1 Transpose of J approaches (easiest to implement) Pseudo inverse of J approaches (allows introducing null space of J) Damped least squares (see Buss review, most stable but slow)

69 Jacobian Transpose Approach Find d s.t. e = J( ) d would mean d = J -1 e Transpose J T : d = J T e scalar needs to be small to reduce magnitude of error e Transpose always exists, but often produces poor quality solutions

70 Jacobian Pseudo Inverse Approach Find d s.t. e = J( ) d would mean d = J -1 e Pseudo inverse J + : d = J + e J + also called Moore-Penrose inverse Gives best solution to J d = e in sense of least squares Has instability issues near singularities A singular value decomposition (SVD) of J gives an easy way to compute J +

71 Jacobian Pseudo Inverse Approach J + has an additional property: I - J J + performs a projection onto the null space of J (self-motion manifold) Null space is space of vectors such that ds = 0 { d J d = 0} has dim = n 6 Any vector of values to joint dofs that minimizes some other objective function (e.g. potential energy of a protein chain) can be projected onto the null space and obtain a vector that minimizes energy and keeps the end effectors in their place

72 Computation of J + from SVD of J 1. SVD decomposition J = U V T where: - U in an 6 6 square orthonormal matrix - V is an n 6 square orthonormal matrix - is of the form diag[ i ]: J + = V + U T where + =diag[1/ i ] Can verify that JJ + = (U V T ) (V + U T ) = I

73 SVD of J Yields Null Space of J J dx U 6 6 V T n dq =

74 SVD of J Yields Null Space of J J dx U 6 6 V T 6 n n n dq = 0 Some singular values will be 0 Corresponding vectors in V T form null space Gram-Schmidt orthogonalization

75 SVD of J Yields Null Space of J J dx U 6 6 V T 6 n n n dq = 0 N T (n-6) basis N of null space

76 Minimization of Objective Function with Closure Input: Chain with ends at target poses Repeat 1. Compute Jacobian matrix J at current q 2. Compute null-space basis N using SVD of J 3. Compute gradient T( ) and y=- T( ) 4. Move along projection NN T y until minimum of T is reached or closure is broken I. Lotan, H. van den Bedem, A.M. Deacon and J.-C Latombe. Computing Protein Structures from Electron Density Maps: The Missing Loop Problem. Proc. 6th Workshop on Algorithmic Foundations of Robotics (WAFR `04)