Robotics. Kinematics. Marc Toussaint University of Stuttgart Winter 2017/18

Robotics Kinematics 3D geometry, homogeneous transformations, kinematic map, Jacobian, inverse kinematics as optimization problem, motion profiles, trajectory interpolation, multiple simultaneous tasks, special task variables, singularities, configuration/operational/null space Marc Toussaint University of Stuttgart Winter 2017/18 Lecturer: Duy Nguyen-Tuong Bosch Center for AI - Research Dept.

Two types of robotics : 1) Mobile robotics is all about localization & mapping 2) Manipulation is all about interacting with the world 0) Kinematic/Dynamic Motion Control: same as 2) without ever making it to interaction.. Typical manipulation robots (and animals) are kinematic trees Their pose/state is described by all joint angles 2/63

Basic motion generation problem Move all joints in a coordinated way so that the endeffector makes a desired movement 01-kinematics:./x.exe -mode 2/3/4 3/63

Outline Basic 3D geometry and notation Kinematics: φ : q y Inverse Kinematics: y q = argmin q φ(q) y 2 C + q q 0 2 W Basic motion heuristics: Motion profiles Additional things to know Many simultaneous task variables Singularities, null space, 4/63

Basic 3D geometry & notation 5/63

Pose (position & orientation) A pose is described by a translation p R 3 and a rotation R SO(3) R is an orthonormal matrix (orthogonal vectors stay orthogonal, unit vectors stay unit) R -1 = R columns and rows are orthogonal unit vectors det(r) = 1 R 11 R 12 R 13 R = R 21 R 22 R 23 R 31 R 32 R 33 6/63

Frame and coordinate transforms Let (o, e 1:3 ) be the world frame, (o, e 1:3) be the body s frame. The new basis vectors are the columns in R, that is, e 1 = R 11 e 1 + R 21 e 2 + R 31 e 3, etc, x = coordinates in world frame (o, e 1:3 ) x = coordinates in body frame (o, e 1:3) p = coordinates of o in world frame (o, e 1:3 ) x = p + Rx 7/63

Briefly: Alternative Rotation Representations See the geometry notes for more details: http://ipvs.informatik.uni-stuttgart.de/mlr/marc/notes/ 3d-geometry.pdf 8/63

Euler angles Describe rotation by consecutive rotation about different axis: 3-1-3 or 3-1-2 conventions, yaw-pitch-roll (3-2-1) in air flight first rotate φ about e 3, then θ about the new e 1, then ψ about the new e 3 Gimbal Lock Euler angles have severe problem: if two axes align: blocks 1 DoF of rotation!! singularity of Euler angles Example: 3-1-3 and second rotation 0 or π 9/63

Rotation vector vector w R 3 length w = θ is rotation angle (in radians) direction of w = rotation axis (w = w/θ) Application on a vector v (Rodrigues formula): Conversion to matrix: w v = cos θ v + sin θ (w v) + (1 cos θ) w(w v) R(w) = exp(ŵ) = cos θ I + sin θ ŵ/θ + (1 cos θ) ww /θ 2 ŵ := 0 w 3 w 2 w 3 0 w 1 w 2 w 1 0 (ŵ is called skew matrix, with property ŵv = w v; exp( ) is called exponential map) Composition: convert to matrix first Drawback: singularity for small rotations 10/63

Quaternion A quaternion is r R 4 with unit length r = r 2 0 + r 2 1 + r 2 2 + r 2 3 = 1 r = r 0 r, r 0 = cos(θ/2), r = sin(θ/2) w where w is the unit length rotation axis and θ is the rotation angle Conversion to matrix R(r) = 1 r 22 r 33 r 12 r 03 r 13 + r 02 r 12 + r 03 1 r 11 r 33 r 23 r 01 r 13 r 02 r 23 + r 01 1 r 11 r 22 r ij = 2r i r j, r 0 = 1 2 1 + trr r 3 = (R 21 R 12 )/(4r 0 ), r 2 = (R 13 R 31 )/(4r 0 ), r 1 = (R 32 R 23 )/(4r 0 ) Composition r r = r 0 r 0 r r r 0 r + r 0 r + r r Application to vector v: convert to matrix first Benefits: fast composition. No sin/cos computations. Use this! 11/63

Homogeneous transformations x A = coordinates of a point in frame A x B = coordinates of a point in frame B Translation and rotation: x A = t + Rx B Homogeneous transform T R 4 4 : T A B = R t 0 1 x A = T A B x B = R t 0 1 x B 1 = RxB + t 1 in homogeneous coordinates, we append a 1 to all coordinate vectors The inverse transform is T B A = T 1 A B = R 1 R 1 t 0 1 12/63

Is T A B forward or backward? T A B describes the translation and rotation of frame B relative to A That is, it describes the forward FRAME transformation (from A to B) T A B describes the coordinate transformation from x B to x A That is, it describes the backward COORDINATE transformation Confused? Vectors (and frames) transform covariant, coordinates contra-variant. See geometry notes or Wikipedia for more details, if you like. 13/63

Composition of transforms T W C = T W A T A B T B C x W = T W A T A B T B C x C 14/63

Kinematics 15/63

Kinematic structure B eff joint transf. A A' B' C' C relative eff. offset link transf. W A kinematic structure is a graph (usually tree or chain) of rigid links and joints T W eff (q) = T W A T A A (q) T A B T B B (q) T B C T C C (q) T C eff 16/63

Joint types Joint transformations: T A A (q) depends on q R n revolute joint: joint angle q R determines rotation about x-axis: T A A (q) = 1 0 0 0 0 cos(q) sin(q) 0 0 sin(q) cos(q) 0 0 0 0 1 prismatic joint: offset q R determines translation along x-axis: T A A (q) = 1 0 0 q 0 1 0 0 0 0 1 0 0 0 0 1 others: screw (1dof), cylindrical (2dof), spherical (3dof), universal (2dof) 17/63

18/63

Kinematic Map For any joint angle vector q R n we can compute T W eff (q) by forward chaining of transformations T W eff (q) gives us the pose of the endeffector in the world frame 19/63

Kinematic Map For any joint angle vector q R n we can compute T W eff (q) by forward chaining of transformations T W eff (q) gives us the pose of the endeffector in the world frame In general, a kinematic map is any (differentiable) mapping φ : q y that maps to some arbitrary feature y R d of the joint vector q R n 19/63

Kinematic Map The three most important examples for a kinematic map φ are A position v on the endeffector transformed to world coordinates: φ pos eff,v (q) = T W eff(q) v R 3 A direction v R 3 attached to the endeffector in world coordinates: φ vec eff,v(q) = R W eff (q) v R 3 Where R A B is the rotation in T A B. The (quaternion) orientation u R 4 of the endeffector: φ quat eff (q) = R W eff(q) R 4 See the technical reference later for more kinematic maps, especially relative position, direction and quaternion maps. 20/63

Jacobian When we change the joint angles, δq, how does the effector position change, δy? Given the kinematic map y = φ(q) and its Jacobian J(q) = q φ(q), we have: δy = J(q) δq J(q) = q φ(q) = φ 1(q) q 1 φ 2(q) q 1. φ d (q) q 1 φ 1(q) q 2... φ 2(q) q 2... φ d (q) q 2... φ 1(q) q n φ 2(q) q n. φ d (q) q n R d n 21/63

Jacobian for a rotational degree of freedom eff axis point i-th joint Assume the i-th joint is located at p i = t W i (q) and has rotation axis 1 a i = R W i (q) 0 0 We consider an infinitesimal variation δq i R of the ith joint and see how an endeffector position p eff = φ pos eff,v (q) and attached vector a eff = φ vec eff,v (q) change. 22/63

Jacobian for a rotational degree of freedom axis eff Consider a variation δq i the whole sub-tree rotates point i-th joint δp eff = [a i (p eff p i )] δq i δa eff = [a i a eff ] δq i Position Jacobian: J pos eff,v (q) = [a1 (peff p1)] [a2 (peff p2)]. [an (peff pn)] R 3 n Vector Jacobian: J vec eff,v(q) = [a1 aeff] [a2 aeff]. [an aeff] R 3 n 23/63

Jacobian for general degrees of freedom Every degree of freedom q i generates (infinitesimally, at a given q) a rotation around axis a i at point p i and/or a translation along the axis b i For instance: the DOF of a hinge joint just creates a rotation around a i at p i the DOF of a prismatic joint creates a translation along b i the DOF of a rolling cylinder creates rotation and translation the first DOF of a cylindrical joint generates a translation, its second DOF a translation We can compute all Jacobians from knowing a i, p i and b i for all DOFs (in the current configuration q R n ) 24/63

Inverse Kinematics 25/63

Inverse Kinematics problem Generally, the aim is to find a robot configuration q such that φ(q) = y Iff φ is invertible q = φ -1 (y ) But in general, φ will not be invertible: 1) The pre-image φ -1 (y ) = may be empty: No configuration can generate the desired y 2) The pre-image φ -1 (y ) may be large: many configurations can generate the desired y 26/63

Inverse Kinematics as optimization problem We formalize the inverse kinematics problem as an optimization problem q = argmin φ(q) y 2 C + q q 0 2 W q The 1st term ensures that we find a configuration even if y is not exactly reachable The 2nd term disambiguates the configurations if there are many φ -1 (y ) 27/63

Inverse Kinematics as optimization problem q = argmin q φ(q) y 2 C + q q 0 2 W The formulation of IK as an optimization problem is very powerful and has many nice properties We will be able to take the limit C, enforcing exact φ(q) = y if possible Non-zero C -1 and W corresponds to a regularization that ensures numeric stability Classical concepts can be derived as special cases: Null-space motion regularization; singularity robutness multiple tasks hierarchical tasks 28/63

Solving Inverse Kinematics The obvious choice of optimization method for this problem is Gauss-Newton, using the Jacobian of φ We first describe just one step of this, which leads to the classical equations for inverse kinematics using the local Jacobian... 29/63

Solution using the local linearization When using the local linearization of φ at q 0, φ(q) y 0 + J (q q 0 ), y 0 = φ(q 0 ) We can derive the optimum as f(q) = φ(q) y 2 C + q q 0 2 W = y 0 y + J (q q 0) 2 C + q q 0 2 W q f(q) = 0 = 2(y 0 y + J (q q 0)) CJ + 2(q q 0) T W J C (y y 0) = (J CJ + W ) (q q 0) q = q 0 + J (y y 0 ) with J = (J CJ + W ) -1 J C = W -1 J (JW -1 J + C -1 ) -1 (Woodbury identity) For C and W = I, J = J (JJ ) -1 is called pseudo-inverse W generalizes the metric in q-space C regularizes this pseudo-inverse (see later section on singularities) 30/63

Small step application This approximate solution to IK makes sense if the local linearization of φ at q 0 is good if q 0 and q are close This equation is therefore typically used to iteratively compute small steps in configuration space q t+1 = q t + J (y t+1 φ(q t )) where the target y t+1 moves smoothly with t 31/63

Example: Iterating IK to follow a trajectory Assume initial posture q 0. We want to reach a desired endeff position y in T steps: Input: initial state q 0, desired y, methods φ pos and J pos Output: trajectory q 0:T 1: Set y 0 = φ pos (q 0 ) // starting endeff position 2: for t = 1 : T do 3: y φ pos (q t-1 ) // current endeff position 4: J J pos (q t-1 ) // current endeff Jacobian 5: ŷ y 0 + (t/t )(y y 0 ) // interpolated endeff target 6: q t = q t-1 + J (ŷ y) // new joint positions 7: Command q t to all robot motors and compute all T W i (q t) 8: end for 01-kinematics:./x.exe -mode 2/3 32/63

Two additional notes What if we linearize at some arbitrary q instead of q 0? φ(q) y + J (q q ), y = φ(q ) q = argmin φ(q) y 2 C + q q + (q q 0) 2 W q = q + J (y y ) + (I J J) h, h = q 0 q (1) What if we want to find the exact (local) optimum? E.g. what if we want to compute a big step (where q will be remote from q) and we cannot rely only on the local linearization approximation? Iterate equation (1) (optionally with a step size < 1 to ensure convergence) by setting the point y of linearization to the current q This is equivalent to the Gauss-Newton algorithm 33/63

Where are we? We ve derived a basic motion generation principle in robotics from an understanding of robot geometry & kinematics a basic notion of optimality 34/63

Where are we? We ve derived a basic motion generation principle in robotics from an understanding of robot geometry & kinematics a basic notion of optimality In the remainder: A. Discussion of classical concepts B. Heuristic motion profiles for simple trajectory generation C. Extension to multiple task variables 34/63

Discussion of classical concepts Singularity and singularity-robustness Nullspace, task/operational space, joint space inverse kinematics motion rate control 35/63

Singularity In general: A matrix J singular rank(j) < d rows of J are linearly dependent dimension of image is < d δy = Jδq dimensions of δy limited Intuition: arm fully stretched 36/63

Singularity In general: A matrix J singular rank(j) < d rows of J are linearly dependent dimension of image is < d δy = Jδq dimensions of δy limited Intuition: arm fully stretched Implications: det(jj ) = 0 pseudo-inverse J (JJ ) -1 is ill-defined! inverse kinematics δq = J (JJ ) -1 δy computes infinite steps! Singularity robust pseudo inverse J (JJ + ɛi) -1 The term ɛi is called regularization Recall our general solution (for W = I) J = J (JJ + C -1 ) -1 is already singularity robust 36/63

Null/task/operational/joint/configuration spaces The space of all q R n is called joint/configuration space The space of all y R d is called task/operational space Usually d < n, which is called redundancy For a desired endeffector state y there exists a whole manifold (assuming φ is smooth) of joint configurations q: nullspace(y ) = {q φ(q) = y } We have δq = argmin q q a 2 W + Jq δy 2 C = J # δy + (I J # J)a, J # = W -1 J (JW -1 J + C -1 ) -1 In the limit C it is guaranteed that Jδq = δy (we are exacty on the manifold). The term a introduces additional nullspace motion. 37/63

Inverse Kinematics and Motion Rate Control Some clarification of concepts: The notion kinematics describes the mapping φ : q y, which usually is a many-to-one function. The notion inverse kinematics in the strict sense describes some mapping g : y q such that φ(g(y)) = y, which usually is non-unique or ill-defined. In practice, one often refers to δq = J δy as inverse kinematics. When iterating δq = J δy in a control cycle with time step τ (typically τ 1 10 msec), then ẏ = δy/τ and q = δq/τ and q = J ẏ. Therefore the control cycle effectively controls the endeffector velocity this is why it is called motion rate control. 38/63

Heuristic motion profiles 39/63

Heuristic motion profiles Assume initially x = 0, ẋ = 0. After 1 second you want x = 1, ẋ = 0. How do you move from x = 0 to x = 1 in one second? The sine profile x t = x 0 + 1 2 [1 cos(πt/t )](x T x 0 ) is a compromise for low max-acceleration and max-velocity Taken from http://www.20sim.com/webhelp/toolboxes/mechatronics_toolbox/ motion_profile_wizard/motionprofiles.htm 40/63

Motion profiles Generally, let s define a motion profile as a mapping MP : [0, 1] [0, 1] with MP(0) = 0 and MP(1) = 1 such that the interpolation is given as x t = x 0 + MP(t/T ) (x T x 0 ) For example MP ramp (s) = s MP sin (s) = 1 [1 cos(πs)] 2 41/63

Joint space interpolation 1) Optimize a desired final configuration q T : Given a desired final task value y T, optimize a final joint state q T to minimize the function f(q T ) = q T q 0 2 W/T + y T φ(q T ) 2 C The metric 1 W is consistent with T cost terms with step metric W. T In this optimization, q T will end up remote from q 0. So we need to iterate Gauss-Newton, as described on slide 33. 2) Compute q 0:T as interpolation between q 0 and q T : Given the initial configuration q 0 and the final q T, interpolate on a straight line with a motion profile. E.g., q t = q 0 + MP(t/T ) (q T q 0) 42/63

Task space interpolation 1) Compute y 0:T as interpolation between y 0 and y T : Given a initial task value y 0 and a desired final task value y T, interpolate on a straight line with a motion profile. E.g, y t = y 0 + MP(t/T ) (y T y 0) 2) Project y 0:T to q 0:T using inverse kinematics: Given the task trajectory y 0:T, compute a corresponding joint trajectory q 0:T using inverse kinematics q t+1 = q t + J (y t+1 φ(q t)) (As steps are small, we should be ok with just using this local linearization.) 43/63

Multiple tasks 44/63

Multiple tasks 45/63

Multiple tasks RightHand position LeftHand position 46/63

Multiple tasks Assume we have m simultaneous tasks; for each task i we have: a kinematic map φ i : R n R d i a current value φ i(q t) a desired value y i a precision ϱ i (equiv. to a task cost metric C i = ϱ i I) Each task contributes a term to the objective function q = argmin q q q 0 2 W + ϱ 1 φ 1 (q) y 1 2 + ϱ 2 φ 2 (q) y 2 2 + 47/63

Multiple tasks Assume we have m simultaneous tasks; for each task i we have: a kinematic map φ i : R n R d i a current value φ i(q t) a desired value y i a precision ϱ i (equiv. to a task cost metric C i = ϱ i I) Each task contributes a term to the objective function q = argmin q which we can also write as q q 0 2 W + ϱ 1 φ 1 (q) y 1 2 + ϱ 2 φ 2 (q) y 2 2 + q = argmin q where Φ(q) := q q 0 2 W + Φ(q) 2 ϱ1 (φ 1 (q) y 1) ϱ2 (φ 2 (q) y2). R i di 47/63

Multiple tasks We can pack together all tasks in one big task Φ. Example: We want to control the 3D position of the left hand and of the right hand. Both are packed to one 6-dimensional task vector which becomes zero if both tasks are fulfilled. The big Φ is scaled/normalized in a way that the desired value is always zero the cost metric is I Using the local linearization of Φ at q 0, J = Φ(q0) q, the optimum is q = argmin q q q 0 2 W + Φ(q) 2 q 0 (J J + W ) -1 J Φ(q 0 ) = q 0 J # Φ(q 0 ) 48/63

Multiple tasks We learnt how to puppeteer a robot We can handle many task variables (but specifying their precisions ϱ i becomes cumbersome...) RightHand position LeftHand position In the remainder: A. Classical limit of hierarchical IK and nullspace motion B. What are interesting task variables? 49/63

Hierarchical IK & nullspace motion In the classical view, tasks should be executed exactly, which means taking the limit ϱ i in some prespecified hierarchical order. We can rewrite the solution in a way that allows for such a hierarchical limit: One task plus nullspace motion : f(q) = q a 2 W + ϱ 1 J 1q y 1 2 q = [W + ϱ 1J 1J 1] -1 [W a + ϱ 1J 1y 1] = J # 1 y 1 + (I J # 1 J 1)a J # 1 = (W/ϱ 1 + J 1J 1) -1 J 1 = W -1 J 1(J 1W -1 J 1 + I/ϱ 1) -1 Two tasks plus nullspace motion : f(q) = q a 2 W + ϱ 1 J 1q y 1 2 + ϱ 2 J 2q y 2 2 q = J # 1 y 1 + (I J # 1 J 1)[J # 2 y 2 + (I J # 2 J 2)a] J # 2 = (W/ϱ 2 + J 2J 2) -1 J 2 = W -1 J 2(J 2W -1 J 2 + I/ϱ 2) -1 etc... 50/63

Hierarchical IK & nullspace motion The previous slide did nothing but rewrite the nice solution q = J # Φ(q 0 ) (for the big Φ) in a strange hierarchical way that allows to see nullspace projection The benefit of this hierarchical way to write the solution is that one can take the hierarchical limit ϱ i and retrieve classical hierarchical IK The drawbacks are: It is somewhat ugly In practise, I would recommend regularization in any case (for numeric stability). Regularization corresponds to NOT taking the full limit ϱ i. Then the hierarchical way to write the solution is unnecessary. (However, it points to a hierarchical regularization, which might be numerically more robust for very small regularization?) The general solution allows for arbitrary blending of tasks 51/63

Reference: interesting task variables The following slides will define 10 different types of task variables. This is meant as a reference and to give an idea of possibilities... 52/63

Position dimension d = 3 parameters kin. map Jacobian Position of some point attached to link i link index i, point offset v φ pos iv (q) = T W i v J pos iv (q) k = [k i] a k (φ pos iv (q) p k) Notation: a k, p k are axis and position of joint k [k i] indicates whether joint k is between root and link i J k is the kth column of J 53/63

Vector dimension d = 3 parameters kin. map Jacobian Vector attached to link i link index i, attached vector v φ vec iv (q) = R W i v J vec iv (q) = A i φ vec iv (q) Notation: A i is a matrix with columns (A i) k = [k i] a k containing the joint axes or zeros the short notation A p means that each column in A takes the cross-product with p. 54/63

Relative position dimension d = 3 parameters kin. map Jacobian Position of a point on link i relative to point on link j link indices i, j, point offset v in i and w in j φ pos iv jw (q) = R-1 j (φpos iv φpos jw ) pos (q) = R-1 j [Jiv J pos jw A j (φ pos iv φpos jw )] J pos iv jw Derivation: For y = Rp the derivative w.r.t. a rotation around axis a is y = Rp + R p = Rp + a Rp. For y = R -1 p the derivative is y = R -1 p R -1 (R )R -1 p = R -1 (p a p). (For details see http://ipvs.informatik.uni-stuttgart.de/mlr/marc/notes/3d-geometry.pdf) 55/63

Relative vector dimension d = 3 Vector attached to link i relative to link j parameters kin. map Jacobian link indices i, j, attached vector v in i φ vec iv j (q) = R-1 j φvec iv J vec iv j vec (q) = R-1 j [Jiv A j φ vec iv ] 56/63

Alignment Alignment of a vector attached to link i with a reference v dimension d = 1 parameters kin. map Jacobian link index i, attached vector v, world reference v φ align iv (q) = v φ vec iv J align iv (q) = v Jiv vec Note: φ align = 1 align φ align = 1 anti-align φ align = 0 orthog. 57/63

Relative Alignment Alignment a vector attached to link i with vector attached to j dimension d = 1 parameters kin. map Jacobian link indices i, j, attached vectors v, w φ align iv jw (q) = (φvec jw ) φ vec iv J align iv jw (q) = (φvec jw ) Jiv vec + φvec iv Jjw vec 58/63

Joint limits Penetration of joint limits dimension d = 1 parameters joint limits q low, q hi, margin m kin. map φ limits (q) = 1 n m i=1 [m q i + q low ] + + [m + q i q hi ] + Jacobian J limits (q) 1,i = 1 m [m q i+q low > 0]+ 1 m [m+q i q hi > 0] [x] + = x > 0?x : 0 [ ]: indicator function 59/63

Collision limits dimension d = 1 Penetration of collision limits parameters margin m kin. map φ col (q) = 1 m K k=1 [m pa k pb k ]+ Jacobian J col (q) = 1 m K k=1 [m pa k pb k > 0] ( J pos p a k + J pos ) p a p b k pb k k p a k pb k A collision detection engine returns a set {(a, b, p a, p b ) K k=1} of potential collisions between link a k and b k, with nearest points p a k on a and p b k on b. 60/63

Center of gravity Center of gravity of the whole kinematic structure dimension d = 3 parameters kin. map Jacobian (none) φ cog (q) = i mass i φ pos ic i J cog (q) = i mass i J pos ic i c i denotes the center-of-mass of link i (in its own frame) 61/63

Homing dimension parameters kin. map Jacobian The joint angles themselves d = n (none) φ qitself (q) = q J qitself (q) = I n Example: Set the target y = 0 and the precision ϱ very low this task describes posture comfortness in terms of deviation from the joints zero position. In the classical view, it induces nullspace motion. 62/63

Task variables conclusions There is much space for creativity in defining task variables! Many are extensions of φ pos and φ vec and the Jacobians combine the basic Jacobians. nearest distance LeftHand position What the right task variables are to design/describe motion is a very hard problem! In what task space do humans control their motion? Possible to learn from data ( task space retrieval ) or perhaps via Reinforcement Learning. In practice: Robot motion design (including grasping) may require cumbersome hand-tuning of such task variables. 63/63