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

Size: px
Start display at page:

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

Transcription

1 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.

2 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

3 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

4 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

5 Basic 3D geometry & notation 5/63

6 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

7 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

8 Briefly: Alternative Rotation Representations See the geometry notes for more details: 3d-geometry.pdf 8/63

9 Euler angles Describe rotation by consecutive rotation about different axis: or 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: and second rotation 0 or π 9/63

10 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

11 Quaternion A quaternion is r R 4 with unit length r = r r r 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 = 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

12 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 /63

13 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

14 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

15 Kinematics 15/63

16 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

17 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) = cos(q) sin(q) 0 0 sin(q) cos(q) prismatic joint: offset q R determines translation along x-axis: T A A (q) = q others: screw (1dof), cylindrical (2dof), spherical (3dof), universal (2dof) 17/63

18 18/63

19 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

20 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

21 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

22 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

23 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

24 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

25 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

26 Inverse Kinematics 25/63

27 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

28 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

29 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

30 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

31 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

32 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

33 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

34 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 Why does this not follow the interpolated trajectory ŷ 0:T exactly? What happens if T = 1 and y is far? 32/63

35 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

36 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

37 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

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

39 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

40 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

41 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 37/63

42 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

43 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

44 Heuristic motion profiles 39/63

45 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 [1 cos(πt/t )](x T x 0 ) is a compromise for low max-acceleration and max-velocity Taken from motion_profile_wizard/motionprofiles.htm 40/63

46 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

47 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

48 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

49 Multiple tasks 44/63

50 Multiple tasks 45/63

51 Multiple tasks RightHand position LeftHand position 46/63

52 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) 47/63

53 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 ϱ 2 φ 2 (q) y /63

54 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 ϱ 2 φ 2 (q) y 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

55 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

56 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

57 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 ϱ 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

58 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

59 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

60 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

61 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

62 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 55/63

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

64 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

65 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

66 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

67 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

68 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

69 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

70 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

Introduction to Robotics

Introduction to Robotics Introduction to Robotics Marc Toussaint April 2016 This is a direct concatenation and reformatting of all lecture slides and exercises from the Robotics course (winter term 2014/15, U Stuttgart), including

More information

Robotics. Dynamics. Marc Toussaint U Stuttgart

Robotics. Dynamics. Marc Toussaint U Stuttgart Robotics Dynamics 1D point mass, damping & oscillation, PID, dynamics of mechanical systems, Euler-Lagrange equation, Newton-Euler recursion, general robot dynamics, joint space control, reference trajectory

More information

MEAM 520. More Velocity Kinematics

MEAM 520. More Velocity Kinematics MEAM 520 More Velocity Kinematics Katherine J. Kuchenbecker, Ph.D. General Robotics, Automation, Sensing, and Perception Lab (GRASP) MEAM Department, SEAS, University of Pennsylvania Lecture 12: October

More information

Position and orientation of rigid bodies

Position and orientation of rigid bodies Robotics 1 Position and orientation of rigid bodies Prof. Alessandro De Luca Robotics 1 1 Position and orientation right-handed orthogonal Reference Frames RF A A p AB B RF B rigid body position: A p AB

More information

Robotics. Dynamics. University of Stuttgart Winter 2018/19

Robotics. Dynamics. University of Stuttgart Winter 2018/19 Robotics Dynamics 1D point mass, damping & oscillation, PID, dynamics of mechanical systems, Euler-Lagrange equation, Newton-Euler, joint space control, reference trajectory following, optimal operational

More information

The Jacobian. Jesse van den Kieboom

The Jacobian. Jesse van den Kieboom The Jacobian Jesse van den Kieboom jesse.vandenkieboom@epfl.ch 1 Introduction 1 1 Introduction The Jacobian is an important concept in robotics. Although the general concept of the Jacobian in robotics

More information

Differential Kinematics

Differential Kinematics Differential Kinematics Relations between motion (velocity) in joint space and motion (linear/angular velocity) in task space (e.g., Cartesian space) Instantaneous velocity mappings can be obtained through

More information

8 Velocity Kinematics

8 Velocity Kinematics 8 Velocity Kinematics Velocity analysis of a robot is divided into forward and inverse velocity kinematics. Having the time rate of joint variables and determination of the Cartesian velocity of end-effector

More information

Robotics I Kinematics, Dynamics and Control of Robotic Manipulators. Velocity Kinematics

Robotics I Kinematics, Dynamics and Control of Robotic Manipulators. Velocity Kinematics Robotics I Kinematics, Dynamics and Control of Robotic Manipulators Velocity Kinematics Dr. Christopher Kitts Director Robotic Systems Laboratory Santa Clara University Velocity Kinematics So far, we ve

More information

Case Study: The Pelican Prototype Robot

Case Study: The Pelican Prototype Robot 5 Case Study: The Pelican Prototype Robot The purpose of this chapter is twofold: first, to present in detail the model of the experimental robot arm of the Robotics lab. from the CICESE Research Center,

More information

Lecture Notes: (Stochastic) Optimal Control

Lecture Notes: (Stochastic) Optimal Control Lecture Notes: (Stochastic) Optimal ontrol Marc Toussaint Machine Learning & Robotics group, TU erlin Franklinstr. 28/29, FR 6-9, 587 erlin, Germany July, 2 Disclaimer: These notes are not meant to be

More information

Inverse differential kinematics Statics and force transformations

Inverse differential kinematics Statics and force transformations Robotics 1 Inverse differential kinematics Statics and force transformations Prof Alessandro De Luca Robotics 1 1 Inversion of differential kinematics! find the joint velocity vector that realizes a desired

More information

ROBOTICS 01PEEQW. Basilio Bona DAUIN Politecnico di Torino

ROBOTICS 01PEEQW. Basilio Bona DAUIN Politecnico di Torino ROBOTICS 01PEEQW Basilio Bona DAUIN Politecnico di Torino Kinematic Functions Kinematic functions Kinematics deals with the study of four functions(called kinematic functions or KFs) that mathematically

More information

(W: 12:05-1:50, 50-N202)

(W: 12:05-1:50, 50-N202) 2016 School of Information Technology and Electrical Engineering at the University of Queensland Schedule of Events Week Date Lecture (W: 12:05-1:50, 50-N202) 1 27-Jul Introduction 2 Representing Position

More information

Lecture Schedule Week Date Lecture (M: 2:05p-3:50, 50-N202)

Lecture Schedule Week Date Lecture (M: 2:05p-3:50, 50-N202) J = x θ τ = J T F 2018 School of Information Technology and Electrical Engineering at the University of Queensland Lecture Schedule Week Date Lecture (M: 2:05p-3:50, 50-N202) 1 23-Jul Introduction + Representing

More information

Advanced Robotic Manipulation

Advanced Robotic Manipulation Advanced Robotic Manipulation Handout CS37A (Spring 017 Solution Set # Problem 1 - Redundant robot control The goal of this problem is to familiarize you with the control of a robot that is redundant with

More information

MCE/EEC 647/747: Robot Dynamics and Control. Lecture 2: Rigid Motions and Homogeneous Transformations

MCE/EEC 647/747: Robot Dynamics and Control. Lecture 2: Rigid Motions and Homogeneous Transformations MCE/EEC 647/747: Robot Dynamics and Control Lecture 2: Rigid Motions and Homogeneous Transformations Reading: SHV Chapter 2 Mechanical Engineering Hanz Richter, PhD MCE503 p.1/22 Representing Points, Vectors

More information

Robot Control Basics CS 685

Robot Control Basics CS 685 Robot Control Basics CS 685 Control basics Use some concepts from control theory to understand and learn how to control robots Control Theory general field studies control and understanding of behavior

More information

Advanced Robotic Manipulation

Advanced Robotic Manipulation Lecture Notes (CS327A) Advanced Robotic Manipulation Oussama Khatib Stanford University Spring 2005 ii c 2005 by Oussama Khatib Contents 1 Spatial Descriptions 1 1.1 Rigid Body Configuration.................

More information

Robotics. Path Planning. University of Stuttgart Winter 2018/19

Robotics. Path Planning. University of Stuttgart Winter 2018/19 Robotics Path Planning Path finding vs. trajectory optimization, local vs. global, Dijkstra, Probabilistic Roadmaps, Rapidly Exploring Random Trees, non-holonomic systems, car system equation, path-finding

More information

DIFFERENTIAL KINEMATICS. Geometric Jacobian. Analytical Jacobian. Kinematic singularities. Kinematic redundancy. Inverse differential kinematics

DIFFERENTIAL KINEMATICS. Geometric Jacobian. Analytical Jacobian. Kinematic singularities. Kinematic redundancy. Inverse differential kinematics DIFFERENTIAL KINEMATICS relationship between joint velocities and end-effector velocities Geometric Jacobian Analytical Jacobian Kinematic singularities Kinematic redundancy Inverse differential kinematics

More information

Lecture «Robot Dynamics» : Kinematics 3

Lecture «Robot Dynamics» : Kinematics 3 Lecture «Robot Dynamics» : Kinematics 3 151-0851-00 V lecture: CAB G11 Tuesday 10:15-12:00, every week exercise: HG G1 Wednesday 8:15-10:00, according to schedule (about every 2nd week) office hour: LEE

More information

Example: RR Robot. Illustrate the column vector of the Jacobian in the space at the end-effector point.

Example: RR Robot. Illustrate the column vector of the Jacobian in the space at the end-effector point. Forward kinematics: X e = c 1 + c 12 Y e = s 1 + s 12 = s 1 s 12 c 1 + c 12, = s 12 c 12 Illustrate the column vector of the Jacobian in the space at the end-effector point. points in the direction perpendicular

More information

Lecture 10. Rigid Body Transformation & C-Space Obstacles. CS 460/560 Introduction to Computational Robotics Fall 2017, Rutgers University

Lecture 10. Rigid Body Transformation & C-Space Obstacles. CS 460/560 Introduction to Computational Robotics Fall 2017, Rutgers University CS 460/560 Introduction to Computational Robotics Fall 017, Rutgers University Lecture 10 Rigid Body Transformation & C-Space Obstacles Instructor: Jingjin Yu Outline Rigid body, links, and joints Task

More information

Robotics. Path Planning. Marc Toussaint U Stuttgart

Robotics. Path Planning. Marc Toussaint U Stuttgart Robotics Path Planning Path finding vs. trajectory optimization, local vs. global, Dijkstra, Probabilistic Roadmaps, Rapidly Exploring Random Trees, non-holonomic systems, car system equation, path-finding

More information

Nonholonomic Constraints Examples

Nonholonomic Constraints Examples Nonholonomic Constraints Examples Basilio Bona DAUIN Politecnico di Torino July 2009 B. Bona (DAUIN) Examples July 2009 1 / 34 Example 1 Given q T = [ x y ] T check that the constraint φ(q) = (2x + siny

More information

In this section of notes, we look at the calculation of forces and torques for a manipulator in two settings:

In this section of notes, we look at the calculation of forces and torques for a manipulator in two settings: Introduction Up to this point we have considered only the kinematics of a manipulator. That is, only the specification of motion without regard to the forces and torques required to cause motion In this

More information

q 1 F m d p q 2 Figure 1: An automated crane with the relevant kinematic and dynamic definitions.

q 1 F m d p q 2 Figure 1: An automated crane with the relevant kinematic and dynamic definitions. Robotics II March 7, 018 Exercise 1 An automated crane can be seen as a mechanical system with two degrees of freedom that moves along a horizontal rail subject to the actuation force F, and that transports

More information

Numerical Methods for Inverse Kinematics

Numerical Methods for Inverse Kinematics Numerical Methods for Inverse Kinematics Niels Joubert, UC Berkeley, CS184 2008-11-25 Inverse Kinematics is used to pose models by specifying endpoints of segments rather than individual joint angles.

More information

Ridig Body Motion Homogeneous Transformations

Ridig Body Motion Homogeneous Transformations Ridig Body Motion Homogeneous Transformations Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna email: claudio.melchiorri@unibo.it C. Melchiorri (DEIS)

More information

Robot Dynamics II: Trajectories & Motion

Robot Dynamics II: Trajectories & Motion Robot Dynamics II: Trajectories & Motion Are We There Yet? METR 4202: Advanced Control & Robotics Dr Surya Singh Lecture # 5 August 23, 2013 metr4202@itee.uq.edu.au http://itee.uq.edu.au/~metr4202/ 2013

More information

Lecture «Robot Dynamics»: Dynamics and Control

Lecture «Robot Dynamics»: Dynamics and Control Lecture «Robot Dynamics»: Dynamics and Control 151-0851-00 V lecture: CAB G11 Tuesday 10:15 12:00, every week exercise: HG E1.2 Wednesday 8:15 10:00, according to schedule (about every 2nd week) Marco

More information

Robotics & Automation. Lecture 06. Serial Kinematic Chain, Forward Kinematics. John T. Wen. September 11, 2008

Robotics & Automation. Lecture 06. Serial Kinematic Chain, Forward Kinematics. John T. Wen. September 11, 2008 Robotics & Automation Lecture 06 Serial Kinematic Chain, Forward Kinematics John T. Wen September 11, 2008 So Far... We have covered rigid body rotational kinematics: representations of SO(3), change of

More information

Advanced Robotic Manipulation

Advanced Robotic Manipulation Advanced Robotic Manipulation Handout CS37A (Spring 017) Solution Set #3 Problem 1 - Inertial properties In this problem, you will explore the inertial properties of a manipulator at its end-effector.

More information

Chapter 2 Coordinate Systems and Transformations

Chapter 2 Coordinate Systems and Transformations Chapter 2 Coordinate Systems and Transformations 2.1 Coordinate Systems This chapter describes the coordinate systems used in depicting the position and orientation (pose) of the aerial robot and its manipulator

More information

Chapter 3 Numerical Methods

Chapter 3 Numerical Methods Chapter 3 Numerical Methods Part 3 3.4 Differential Algebraic Systems 3.5 Integration of Differential Equations 1 Outline 3.4 Differential Algebraic Systems 3.4.1 Constrained Dynamics 3.4.2 First and Second

More information

Lecture Note 7: Velocity Kinematics and Jacobian

Lecture Note 7: Velocity Kinematics and Jacobian ECE5463: Introduction to Robotics Lecture Note 7: Velocity Kinematics and Jacobian Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018

More information

5. Nonholonomic constraint Mechanics of Manipulation

5. Nonholonomic constraint Mechanics of Manipulation 5. Nonholonomic constraint Mechanics of Manipulation Matt Mason matt.mason@cs.cmu.edu http://www.cs.cmu.edu/~mason Carnegie Mellon Lecture 5. Mechanics of Manipulation p.1 Lecture 5. Nonholonomic constraint.

More information

Ch. 5: Jacobian. 5.1 Introduction

Ch. 5: Jacobian. 5.1 Introduction 5.1 Introduction relationship between the end effector velocity and the joint rates differentiate the kinematic relationships to obtain the velocity relationship Jacobian matrix closely related to the

More information

Robotics I. April 1, the motion starts and ends with zero Cartesian velocity and acceleration;

Robotics I. April 1, the motion starts and ends with zero Cartesian velocity and acceleration; Robotics I April, 6 Consider a planar R robot with links of length l = and l =.5. he end-effector should move smoothly from an initial point p in to a final point p fin in the robot workspace so that the

More information

Lecture «Robot Dynamics»: Dynamics 2

Lecture «Robot Dynamics»: Dynamics 2 Lecture «Robot Dynamics»: Dynamics 2 151-0851-00 V lecture: CAB G11 Tuesday 10:15 12:00, every week exercise: HG E1.2 Wednesday 8:15 10:00, according to schedule (about every 2nd week) office hour: LEE

More information

Robotics I. February 6, 2014

Robotics I. February 6, 2014 Robotics I February 6, 214 Exercise 1 A pan-tilt 1 camera sensor, such as the commercial webcams in Fig. 1, is mounted on the fixed base of a robot manipulator and is used for pointing at a (point-wise)

More information

Lecture Note 8: Inverse Kinematics

Lecture Note 8: Inverse Kinematics ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 8 (ECE5463

More information

Chapter 3 + some notes on counting the number of degrees of freedom

Chapter 3 + some notes on counting the number of degrees of freedom Chapter 3 + some notes on counting the number of degrees of freedom Minimum number of independent parameters = Some number of dependent parameters minus the number of relationships (equations) you can

More information

Lecture Note 12: Dynamics of Open Chains: Lagrangian Formulation

Lecture Note 12: Dynamics of Open Chains: Lagrangian Formulation ECE5463: Introduction to Robotics Lecture Note 12: Dynamics of Open Chains: Lagrangian Formulation Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio,

More information

Lie Groups for 2D and 3D Transformations

Lie Groups for 2D and 3D Transformations Lie Groups for 2D and 3D Transformations Ethan Eade Updated May 20, 2017 * 1 Introduction This document derives useful formulae for working with the Lie groups that represent transformations in 2D and

More information

Minimal representations of orientation

Minimal representations of orientation Robotics 1 Minimal representations of orientation (Euler and roll-pitch-yaw angles) Homogeneous transformations Prof. lessandro De Luca Robotics 1 1 Minimal representations rotation matrices: 9 elements

More information

Manipulator Dynamics 2. Instructor: Jacob Rosen Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA

Manipulator Dynamics 2. Instructor: Jacob Rosen Advanced Robotic - MAE 263D - Department of Mechanical & Aerospace Engineering - UCLA Manipulator Dynamics 2 Forward Dynamics Problem Given: Joint torques and links geometry, mass, inertia, friction Compute: Angular acceleration of the links (solve differential equations) Solution Dynamic

More information

Lecture Note 7: Velocity Kinematics and Jacobian

Lecture Note 7: Velocity Kinematics and Jacobian ECE5463: Introduction to Robotics Lecture Note 7: Velocity Kinematics and Jacobian Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018

More information

Lesson Rigid Body Dynamics

Lesson Rigid Body Dynamics Lesson 8 Rigid Body Dynamics Lesson 8 Outline Problem definition and motivations Dynamics of rigid bodies The equation of unconstrained motion (ODE) User and time control Demos / tools / libs Rigid Body

More information

Robotics. Control Theory. Marc Toussaint U Stuttgart

Robotics. Control Theory. Marc Toussaint U Stuttgart Robotics Control Theory Topics in control theory, optimal control, HJB equation, infinite horizon case, Linear-Quadratic optimal control, Riccati equations (differential, algebraic, discrete-time), controllability,

More information

Robotics I June 11, 2018

Robotics I June 11, 2018 Exercise 1 Robotics I June 11 018 Consider the planar R robot in Fig. 1 having a L-shaped second link. A frame RF e is attached to the gripper mounted on the robot end effector. A B y e C x e Figure 1:

More information

Robot Dynamics Instantaneous Kinematiccs and Jacobians

Robot Dynamics Instantaneous Kinematiccs and Jacobians Robot Dynamics Instantaneous Kinematiccs and Jacobians 151-0851-00 V Lecture: Tuesday 10:15 12:00 CAB G11 Exercise: Tuesday 14:15 16:00 every 2nd week Marco Hutter, Michael Blösch, Roland Siegwart, Konrad

More information

Kinematics of a UR5. Rasmus Skovgaard Andersen Aalborg University

Kinematics of a UR5. Rasmus Skovgaard Andersen Aalborg University Kinematics of a UR5 May 3, 28 Rasmus Skovgaard Andersen Aalborg University Contents Introduction.................................... Notation.................................. 2 Forward Kinematics for

More information

Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik. Robot Dynamics. Dr.-Ing. John Nassour J.

Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik. Robot Dynamics. Dr.-Ing. John Nassour J. Artificial Intelligence & Neuro Cognitive Systems Fakultät für Informatik Robot Dynamics Dr.-Ing. John Nassour 25.1.218 J.Nassour 1 Introduction Dynamics concerns the motion of bodies Includes Kinematics

More information

Lecture Note 4: General Rigid Body Motion

Lecture Note 4: General Rigid Body Motion ECE5463: Introduction to Robotics Lecture Note 4: General Rigid Body Motion Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture

More information

Robot Dynamics Lecture Notes. Robotic Systems Lab, ETH Zurich

Robot Dynamics Lecture Notes. Robotic Systems Lab, ETH Zurich Robot Dynamics Lecture Notes Robotic Systems Lab, ETH Zurich HS 217 Contents 1 Introduction 1 1.1 Nomenclature.............................. 2 1.2 Operators................................ 3 2 Kinematics

More information

Linear Algebra and Robot Modeling

Linear Algebra and Robot Modeling Linear Algebra and Robot Modeling Nathan Ratliff Abstract Linear algebra is fundamental to robot modeling, control, and optimization. This document reviews some of the basic kinematic equations and uses

More information

Trajectory-tracking control of a planar 3-RRR parallel manipulator

Trajectory-tracking control of a planar 3-RRR parallel manipulator Trajectory-tracking control of a planar 3-RRR parallel manipulator Chaman Nasa and Sandipan Bandyopadhyay Department of Engineering Design Indian Institute of Technology Madras Chennai, India Abstract

More information

ME 115(b): Homework #2 Solution. Part (b): Using the Product of Exponentials approach, the structure equations take the form:

ME 115(b): Homework #2 Solution. Part (b): Using the Product of Exponentials approach, the structure equations take the form: ME 5(b): Homework #2 Solution Problem : Problem 2 (d,e), Chapter 3 of MLS. Let s review some material from the parts (b) of this problem: Part (b): Using the Product of Exponentials approach, the structure

More information

Screw Theory and its Applications in Robotics

Screw Theory and its Applications in Robotics Screw Theory and its Applications in Robotics Marco Carricato Group of Robotics, Automation and Biomechanics University of Bologna Italy IFAC 2017 World Congress, Toulouse, France Table of Contents 1.

More information

, respectively to the inverse and the inverse differential problem. Check the correctness of the obtained results. Exercise 2 y P 2 P 1.

, respectively to the inverse and the inverse differential problem. Check the correctness of the obtained results. Exercise 2 y P 2 P 1. Robotics I July 8 Exercise Define the orientation of a rigid body in the 3D space through three rotations by the angles α β and γ around three fixed axes in the sequence Y X and Z and determine the associated

More information

Homogeneous Transformations

Homogeneous Transformations Purpose: Homogeneous Transformations The purpose of this chapter is to introduce you to the Homogeneous Transformation. This simple 4 x 4 transformation is used in the geometry engines of CAD systems and

More information

ROBOTICS: ADVANCED CONCEPTS & ANALYSIS

ROBOTICS: ADVANCED CONCEPTS & ANALYSIS ROBOTICS: ADVANCED CONCEPTS & ANALYSIS MODULE 5 VELOCITY AND STATIC ANALYSIS OF MANIPULATORS Ashitava Ghosal 1 1 Department of Mechanical Engineering & Centre for Product Design and Manufacture Indian

More information

DYNAMICS OF PARALLEL MANIPULATOR

DYNAMICS OF PARALLEL MANIPULATOR DYNAMICS OF PARALLEL MANIPULATOR PARALLEL MANIPULATORS 6-degree of Freedom Flight Simulator BACKGROUND Platform-type parallel mechanisms 6-DOF MANIPULATORS INTRODUCTION Under alternative robotic mechanical

More information

DYNAMICS OF PARALLEL MANIPULATOR

DYNAMICS OF PARALLEL MANIPULATOR DYNAMICS OF PARALLEL MANIPULATOR The 6nx6n matrices of manipulator mass M and manipulator angular velocity W are introduced below: M = diag M 1, M 2,, M n W = diag (W 1, W 2,, W n ) From this definitions

More information

Reconstructing Null-space Policies Subject to Dynamic Task Constraints in Redundant Manipulators

Reconstructing Null-space Policies Subject to Dynamic Task Constraints in Redundant Manipulators Reconstructing Null-space Policies Subject to Dynamic Task Constraints in Redundant Manipulators Matthew Howard Sethu Vijayakumar September, 7 Abstract We consider the problem of direct policy learning

More information

Lecture Notes Multibody Dynamics B, wb1413

Lecture Notes Multibody Dynamics B, wb1413 Lecture Notes Multibody Dynamics B, wb1413 A. L. Schwab & Guido M.J. Delhaes Laboratory for Engineering Mechanics Mechanical Engineering Delft University of Technolgy The Netherlands June 9, 29 Contents

More information

Trajectory Planning from Multibody System Dynamics

Trajectory Planning from Multibody System Dynamics Trajectory Planning from Multibody System Dynamics Pierangelo Masarati Politecnico di Milano Dipartimento di Ingegneria Aerospaziale Manipulators 2 Manipulator: chain of

More information

Robotics & Automation. Lecture 17. Manipulability Ellipsoid, Singularities of Serial Arm. John T. Wen. October 14, 2008

Robotics & Automation. Lecture 17. Manipulability Ellipsoid, Singularities of Serial Arm. John T. Wen. October 14, 2008 Robotics & Automation Lecture 17 Manipulability Ellipsoid, Singularities of Serial Arm John T. Wen October 14, 2008 Jacobian Singularity rank(j) = dimension of manipulability ellipsoid = # of independent

More information

Robotics & Automation. Lecture 25. Dynamics of Constrained Systems, Dynamic Control. John T. Wen. April 26, 2007

Robotics & Automation. Lecture 25. Dynamics of Constrained Systems, Dynamic Control. John T. Wen. April 26, 2007 Robotics & Automation Lecture 25 Dynamics of Constrained Systems, Dynamic Control John T. Wen April 26, 2007 Last Time Order N Forward Dynamics (3-sweep algorithm) Factorization perspective: causal-anticausal

More information

Kinematic representation! Iterative methods! Optimization methods

Kinematic representation! Iterative methods! Optimization methods Human Kinematics Kinematic representation! Iterative methods! Optimization methods Kinematics Forward kinematics! given a joint configuration, what is the position of an end point on the structure?! Inverse

More information

Advanced Dynamics. - Lecture 4 Lagrange Equations. Paolo Tiso Spring Semester 2017 ETH Zürich

Advanced Dynamics. - Lecture 4 Lagrange Equations. Paolo Tiso Spring Semester 2017 ETH Zürich Advanced Dynamics - Lecture 4 Lagrange Equations Paolo Tiso Spring Semester 2017 ETH Zürich LECTURE OBJECTIVES 1. Derive the Lagrange equations of a system of particles; 2. Show that the equation of motion

More information

Lecture Note 8: Inverse Kinematics

Lecture Note 8: Inverse Kinematics ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 8 (ECE5463

More information

Augmented Reality VU numerical optimization algorithms. Prof. Vincent Lepetit

Augmented Reality VU numerical optimization algorithms. Prof. Vincent Lepetit Augmented Reality VU numerical optimization algorithms Prof. Vincent Lepetit P3P: can exploit only 3 correspondences; DLT: difficult to exploit the knowledge of the internal parameters correctly, and the

More information

CS273: Algorithms for Structure Handout # 2 and Motion in Biology Stanford University Thursday, 1 April 2004

CS273: Algorithms for Structure Handout # 2 and Motion in Biology Stanford University Thursday, 1 April 2004 CS273: Algorithms for Structure Handout # 2 and Motion in Biology Stanford University Thursday, 1 April 2004 Lecture #2: 1 April 2004 Topics: Kinematics : Concepts and Results Kinematics of Ligands and

More information

Institute of Geometry, Graz, University of Technology Mobile Robots. Lecture notes of the kinematic part of the lecture

Institute of Geometry, Graz, University of Technology   Mobile Robots. Lecture notes of the kinematic part of the lecture Institute of Geometry, Graz, University of Technology www.geometrie.tugraz.at Institute of Geometry Mobile Robots Lecture notes of the kinematic part of the lecture Anton Gfrerrer nd Edition 4 . Contents

More information

Lecture 7: Kinematics: Velocity Kinematics - the Jacobian

Lecture 7: Kinematics: Velocity Kinematics - the Jacobian Lecture 7: Kinematics: Velocity Kinematics - the Jacobian Manipulator Jacobian c Anton Shiriaev. 5EL158: Lecture 7 p. 1/?? Lecture 7: Kinematics: Velocity Kinematics - the Jacobian Manipulator Jacobian

More information

Operational Space Control of Constrained and Underactuated Systems

Operational Space Control of Constrained and Underactuated Systems Robotics: Science and Systems 2 Los Angeles, CA, USA, June 27-3, 2 Operational Space Control of Constrained and Underactuated Systems Michael Mistry Disney Research Pittsburgh 472 Forbes Ave., Suite Pittsburgh,

More information

Chapter 1. Rigid Body Kinematics. 1.1 Introduction

Chapter 1. Rigid Body Kinematics. 1.1 Introduction Chapter 1 Rigid Body Kinematics 1.1 Introduction This chapter builds up the basic language and tools to describe the motion of a rigid body this is called rigid body kinematics. This material will be the

More information

Robotics I. June 6, 2017

Robotics I. June 6, 2017 Robotics I June 6, 217 Exercise 1 Consider the planar PRPR manipulator in Fig. 1. The joint variables defined therein are those used by the manufacturer and do not correspond necessarily to a Denavit-Hartenberg

More information

Non-holonomic constraint example A unicycle

Non-holonomic constraint example A unicycle Non-holonomic constraint example A unicycle A unicycle (in gray) moves on a plane; its motion is given by three coordinates: position x, y and orientation θ. The instantaneous velocity v = [ ẋ ẏ ] is along

More information

Chapter 6. Screw theory for instantaneous kinematics. 6.1 Introduction. 6.2 Exponential coordinates for rotation

Chapter 6. Screw theory for instantaneous kinematics. 6.1 Introduction. 6.2 Exponential coordinates for rotation Screw theory for instantaneous kinematics 6.1 Introduction Chapter 6 Screw theory was developed by Sir Robert Stawell Ball [111] in 1876, for application in kinematics and statics of mechanisms (rigid

More information

Mechanics, Heat, Oscillations and Waves Prof. V. Balakrishnan Department of Physics Indian Institute of Technology, Madras

Mechanics, Heat, Oscillations and Waves Prof. V. Balakrishnan Department of Physics Indian Institute of Technology, Madras Mechanics, Heat, Oscillations and Waves Prof. V. Balakrishnan Department of Physics Indian Institute of Technology, Madras Lecture 08 Vectors in a Plane, Scalars & Pseudoscalers Let us continue today with

More information

Control of Mobile Robots

Control of Mobile Robots Control of Mobile Robots Regulation and trajectory tracking Prof. Luca Bascetta (luca.bascetta@polimi.it) Politecnico di Milano Dipartimento di Elettronica, Informazione e Bioingegneria Organization and

More information

Given U, V, x and θ perform the following steps: a) Find the rotation angle, φ, by which u 1 is rotated in relation to x 1

Given U, V, x and θ perform the following steps: a) Find the rotation angle, φ, by which u 1 is rotated in relation to x 1 1 The Jacobian can be expressed in an arbitrary frame, such as the base frame located at the first joint, the hand frame located at the end-effector, or the global frame located somewhere else. The SVD

More information

Lecture «Robot Dynamics»: Kinematics 2

Lecture «Robot Dynamics»: Kinematics 2 Lecture «Robot Dynamics»: Kinematics 2 151-0851-00 V lecture: CAB G11 Tuesday 10:15 12:00, every week exercise: HG E1.2 Wednesday 8:15 10:00, according to schedule (about every 2nd week) Marco Hutter,

More information

Physics 411 Lecture 7. Tensors. Lecture 7. Physics 411 Classical Mechanics II

Physics 411 Lecture 7. Tensors. Lecture 7. Physics 411 Classical Mechanics II Physics 411 Lecture 7 Tensors Lecture 7 Physics 411 Classical Mechanics II September 12th 2007 In Electrodynamics, the implicit law governing the motion of particles is F α = m ẍ α. This is also true,

More information

Introduction to Robotics

Introduction to Robotics J. Zhang, L. Einig 277 / 307 MIN Faculty Department of Informatics Lecture 8 Jianwei Zhang, Lasse Einig [zhang, einig]@informatik.uni-hamburg.de University of Hamburg Faculty of Mathematics, Informatics

More information

Rotational & Rigid-Body Mechanics. Lectures 3+4

Rotational & Rigid-Body Mechanics. Lectures 3+4 Rotational & Rigid-Body Mechanics Lectures 3+4 Rotational Motion So far: point objects moving through a trajectory. Next: moving actual dimensional objects and rotating them. 2 Circular Motion - Definitions

More information

Computer Graphics II

Computer Graphics II Computer Graphics II Autumn 2017-2018 Outline 1 3-D Rotations (contd.) Outline 3-D Rotations (contd.) 1 3-D Rotations (contd.) Amalgamating Steps 2, 3 & 4 Quaternions are a more compact way of solving

More information

RECURSIVE INVERSE DYNAMICS

RECURSIVE INVERSE DYNAMICS We assume at the outset that the manipulator under study is of the serial type with n+1 links including the base link and n joints of either the revolute or the prismatic type. The underlying algorithm

More information

Orthogonality. 6.1 Orthogonal Vectors and Subspaces. Chapter 6

Orthogonality. 6.1 Orthogonal Vectors and Subspaces. Chapter 6 Chapter 6 Orthogonality 6.1 Orthogonal Vectors and Subspaces Recall that if nonzero vectors x, y R n are linearly independent then the subspace of all vectors αx + βy, α, β R (the space spanned by x and

More information

Gain Scheduling Control with Multi-loop PID for 2-DOF Arm Robot Trajectory Control

Gain Scheduling Control with Multi-loop PID for 2-DOF Arm Robot Trajectory Control Gain Scheduling Control with Multi-loop PID for 2-DOF Arm Robot Trajectory Control Khaled M. Helal, 2 Mostafa R.A. Atia, 3 Mohamed I. Abu El-Sebah, 2 Mechanical Engineering Department ARAB ACADEMY FOR

More information

Robotics 1 Inverse kinematics

Robotics 1 Inverse kinematics Robotics 1 Inverse kinematics Prof. Alessandro De Luca Robotics 1 1 Inverse kinematics what are we looking for? direct kinematics is always unique; how about inverse kinematics for this 6R robot? Robotics

More information

CONSTANT KINETIC ENERGY ROBOT TRAJECTORY PLANNING

CONSTANT KINETIC ENERGY ROBOT TRAJECTORY PLANNING PERIODICA POLYTECHNICA SER. MECH. ENG. VOL. 43, NO. 2, PP. 213 228 (1999) CONSTANT KINETIC ENERGY ROBOT TRAJECTORY PLANNING Zoltán ZOLLER and Peter ZENTAY Department of Manufacturing Engineering Technical

More information

CS-184: Computer Graphics

CS-184: Computer Graphics CS-184: Computer Graphics Lecture #25: Rigid Body Simulations Tobias Pfaff 537 Soda (Visual Computing Lab) tpfaff@berkeley.edu Reminder Final project presentations next week! Game Physics Types of Materials

More information

ARW Lecture 04 Point Tracking

ARW Lecture 04 Point Tracking ARW Lecture 04 Point Tracking Instructor: Chris Clark Semester: Summer 2016 1 Figures courtesy of Siegwart & Nourbakhsh Planning Based Control Prior Knowledge Operator Commands Localization Path Planning

More information

Least-Squares Fitting of Model Parameters to Experimental Data

Least-Squares Fitting of Model Parameters to Experimental Data Least-Squares Fitting of Model Parameters to Experimental Data Div. of Mathematical Sciences, Dept of Engineering Sciences and Mathematics, LTU, room E193 Outline of talk What about Science and Scientific

More information

Robotics 1 Inverse kinematics

Robotics 1 Inverse kinematics Robotics 1 Inverse kinematics Prof. Alessandro De Luca Robotics 1 1 Inverse kinematics what are we looking for? direct kinematics is always unique; how about inverse kinematics for this 6R robot? Robotics

More information